From 73f47f69b5465343a36cf899d027d12ae057e98c Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Mon, 7 Mar 2022 11:48:00 -0500 Subject: [PATCH 001/165] Update config.yml --- .circleci/config.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.circleci/config.yml b/.circleci/config.yml index 30644d0592..a3db067c90 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -119,6 +119,7 @@ jobs: echo "Target Branch = ${TARGET_BRANCH}" cd src/CARMAPlatform git fetch --force origin ${TARGET_BRANCH}:${TARGET_BRANCH} + ls -lha /opt/carma/coverage_reports/gcov/ sonar-scanner -Dproject.settings=.sonarqube/sonar-scanner.properties -Dsonar.login=${SONAR_SCANNER_TOKEN} -Dsonar.pullrequest.base=${TARGET_BRANCH} -Dsonar.pullrequest.branch=${CIRCLE_BRANCH} -Dsonar.pullrequest.key=${PR_NUM} - store_test_results: path: ~/junit From 5a2df287fce0caeb0183782687fe27f6d79d31fe Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Mon, 7 Mar 2022 13:10:10 -0500 Subject: [PATCH 002/165] listed out ls coverage_reports dir to check reports in folder --- .circleci/config.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index a3db067c90..6afaab06f8 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -69,6 +69,7 @@ jobs: source ${INIT_ENV} export ROS_PARALLEL_JOBS='-j4 -l4' # Try to reduce memory consumption on build bash make_with_coverage.bash -t -e /opt/carma/ -o ./coverage_reports/gcov + ls -la /opt/carma/coverage_reports/gcov/ - run: name: Cleanup before ROS 2 build # Clear the build and install folders before building ROS 2 @@ -88,6 +89,7 @@ jobs: source ${INIT_ENV} source ${ROS_2_ENV} bash make_with_coverage.bash -t -e /opt/carma/ -o ./coverage_reports/gcov + ls -la /opt/carma/coverage_reports/gcov/ # Run SonarCloud analysis # PR Branches and number extracted from Circle variables and github api @@ -119,7 +121,7 @@ jobs: echo "Target Branch = ${TARGET_BRANCH}" cd src/CARMAPlatform git fetch --force origin ${TARGET_BRANCH}:${TARGET_BRANCH} - ls -lha /opt/carma/coverage_reports/gcov/ + ls -la /opt/carma/coverage_reports/gcov/ sonar-scanner -Dproject.settings=.sonarqube/sonar-scanner.properties -Dsonar.login=${SONAR_SCANNER_TOKEN} -Dsonar.pullrequest.base=${TARGET_BRANCH} -Dsonar.pullrequest.branch=${CIRCLE_BRANCH} -Dsonar.pullrequest.key=${PR_NUM} - store_test_results: path: ~/junit From 55b83aa639851e9fd04327f1cc41d44d75a9fe57 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Mon, 23 May 2022 14:27:01 -0700 Subject: [PATCH 003/165] WIP plugin base library --- carma_guidance_plugin/CMakeLists.txt | 72 ++++++++++++++ carma_guidance_plugin/README.md | 3 + carma_guidance_plugin/config/parameters.yaml | 3 + .../carma_guidance_plugin/control_plugin.hpp | 90 ++++++++++++++++++ .../plugin_base_node.hpp | 83 +++++++++++++++++ .../strategic_plugin.hpp | 70 ++++++++++++++ .../carma_guidance_plugin/tactical_plugin.hpp | 70 ++++++++++++++ .../launch/carma_guidance_plugin_launch.py | 68 ++++++++++++++ carma_guidance_plugin/package.xml | 43 +++++++++ carma_guidance_plugin/src/control_plugin.cpp | 93 +++++++++++++++++++ carma_guidance_plugin/src/main.cpp | 33 +++++++ .../src/plugin_base_node.cpp | 93 +++++++++++++++++++ .../src/strategic_plugin.cpp | 67 +++++++++++++ carma_guidance_plugin/src/tactical_plugin.cpp | 67 +++++++++++++ carma_guidance_plugin/test/node_test.cpp | 55 +++++++++++ 15 files changed, 910 insertions(+) create mode 100644 carma_guidance_plugin/CMakeLists.txt create mode 100644 carma_guidance_plugin/README.md create mode 100644 carma_guidance_plugin/config/parameters.yaml create mode 100644 carma_guidance_plugin/include/carma_guidance_plugin/control_plugin.hpp create mode 100644 carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp create mode 100644 carma_guidance_plugin/include/carma_guidance_plugin/strategic_plugin.hpp create mode 100644 carma_guidance_plugin/include/carma_guidance_plugin/tactical_plugin.hpp create mode 100644 carma_guidance_plugin/launch/carma_guidance_plugin_launch.py create mode 100644 carma_guidance_plugin/package.xml create mode 100644 carma_guidance_plugin/src/control_plugin.cpp create mode 100644 carma_guidance_plugin/src/main.cpp create mode 100644 carma_guidance_plugin/src/plugin_base_node.cpp create mode 100644 carma_guidance_plugin/src/strategic_plugin.cpp create mode 100644 carma_guidance_plugin/src/tactical_plugin.cpp create mode 100644 carma_guidance_plugin/test/node_test.cpp diff --git a/carma_guidance_plugin/CMakeLists.txt b/carma_guidance_plugin/CMakeLists.txt new file mode 100644 index 0000000000..e4b8cc18b8 --- /dev/null +++ b/carma_guidance_plugin/CMakeLists.txt @@ -0,0 +1,72 @@ + +# 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. + +cmake_minimum_required(VERSION 3.5) +project(carma_guidance_plugin) + +# Declare carma package and check ROS version +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(2) +carma_package() + +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Name build targets +set(node_exec carma_guidance_plugin_node_exec) +set(node_lib carma_guidance_plugin_node) + +# Includes +include_directories( + include +) + +# Build +ament_auto_add_library(${node_lib} SHARED + src/carma_guidance_plugin_node.cpp +) + +ament_auto_add_executable(${node_exec} + src/main.cpp +) + +# Register component +rclcpp_components_register_nodes(${node_lib} "carma_guidance_plugin::PluginBaseNode") + +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} +) + +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(test_carma_guidance_plugin test/node_test.cpp) + + ament_target_dependencies(test_carma_guidance_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_carma_guidance_plugin ${node_lib}) + +endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/carma_guidance_plugin/README.md b/carma_guidance_plugin/README.md new file mode 100644 index 0000000000..e86518b063 --- /dev/null +++ b/carma_guidance_plugin/README.md @@ -0,0 +1,3 @@ +# carma_guidance_plugin + +TODO for USER: Add description of package and link to confluence documentation. diff --git a/carma_guidance_plugin/config/parameters.yaml b/carma_guidance_plugin/config/parameters.yaml new file mode 100644 index 0000000000..5b061717c3 --- /dev/null +++ b/carma_guidance_plugin/config/parameters.yaml @@ -0,0 +1,3 @@ +# String: Example Parameter +# TODO for USER: Add your own parameters +example_param : "My example value" diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/control_plugin.hpp b/carma_guidance_plugin/include/carma_guidance_plugin/control_plugin.hpp new file mode 100644 index 0000000000..42252d9fd6 --- /dev/null +++ b/carma_guidance_plugin/include/carma_guidance_plugin/control_plugin.hpp @@ -0,0 +1,90 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +namespace carma_guidance_plugin +{ + + /** + * \brief ControlPlugin provides default functionality for all carma guidance plugins. + * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery + * + */ + class ControlPlugin : public PluginBaseNode + { + + private: + // Subscriptions + carma_ros2_utils::SubPtr current_pose_sub_; + carma_ros2_utils::SubPtr current_velocity_sub_; + carma_ros2_utils::SubPtr trajectory_plan_sub_; + + // Timers + rclcpp::TimerBase::SharedPtr command_timer_; + + + protected: + + //! The most recent pose message received by this node + boost::optional current_pose_; + + //! The most recent velocity message received by this node + // NOTE: Only the twist.linear.x and header are guaranteed to be populated + boost::optional current_twist_; + + // TODO comments, note user can determine if trajectory has changed based on trajectory_id + boost::optional current_trajectory_; + + + public: + /** + * \brief ControlPlugin constructor + */ + explicit ControlPlugin(const rclcpp::NodeOptions &); + + // TODO comments + void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg); + void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg); + void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg); + + + virtual void generate_command() = 0; + + //// + // Overrides + //// + + // Non-Final + std::string get_capability() override; + + // Final + uint8_t get_type() override final; + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override final; + }; + +} // carma_guidance_plugin diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp b/carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp new file mode 100644 index 0000000000..f79079a66d --- /dev/null +++ b/carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp @@ -0,0 +1,83 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +namespace carma_guidance_plugin +{ + + /** + * \brief PluginBaseNode provides default functionality for all carma guidance plugins. + * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery + * + */ + class PluginBaseNode : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Publishers + carma_ros2_utils::PubPtr plugin_discovery_pub_; + + // Timers + rclcpp::TimerBase::SharedPtr discovert_timer_; + + public: + /** + * \brief PluginBaseNode constructor + */ + explicit PluginBaseNode(const rclcpp::NodeOptions &); + + // TODO do I need the virtual destructor? + + /** + * \brief Callback for the plugin discovery timer which will publish the plugin discovery message + */ + void discovery_timer_callback(); + + // TODO comments + bool get_activation_status(); + + virtual uint8_t get_type(); + + virtual bool get_availability() = 0; + virtual std::string get_capability() = 0; + virtual std::string get_name() = 0; + virtual std::string get_version_id() = 0; + + virtual carma_ros2_utils::CallbackReturn configure_plugin() = 0; + virtual carma_ros2_utils::CallbackReturn activate_plugin() = 0; + virtual carma_ros2_utils::CallbackReturn deactivate_plugin() = 0; + + //// + // Overrides + //// + // For simplicity of managing the plugin state machine all lifecycle callbacks are implemented as final by the core extending classes (strategic, tactical, control) + // Plugins will use the simplified plugin callbacks via the template pattern + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override; + + }; + +} // carma_guidance_plugin diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/strategic_plugin.hpp b/carma_guidance_plugin/include/carma_guidance_plugin/strategic_plugin.hpp new file mode 100644 index 0000000000..b830e129b4 --- /dev/null +++ b/carma_guidance_plugin/include/carma_guidance_plugin/strategic_plugin.hpp @@ -0,0 +1,70 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +namespace carma_guidance_plugin +{ + + /** + * \brief StrategicPlugin provides default functionality for all carma guidance plugins. + * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery + * + */ + class StrategicPlugin : public PluginBaseNode + { + + private: + // Services + carma_ros2_utils::ServicePtr plan_maneuvers_service_; + + public: + /** + * \brief StrategicPlugin constructor + */ + explicit StrategicPlugin(const rclcpp::NodeOptions &); + + // TODO comments + virtual void plan_maneuvers_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request &req, + carma_planning_msgs::srv::PlanManeuvers::Response &resp) = 0; + + + //// + // Overrides + //// + + // Non-Final + std::string get_capability() override; + + // Final + uint8_t get_type() override final; + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override final; + }; + +} // carma_guidance_plugin diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/tactical_plugin.hpp b/carma_guidance_plugin/include/carma_guidance_plugin/tactical_plugin.hpp new file mode 100644 index 0000000000..5a8bc018f6 --- /dev/null +++ b/carma_guidance_plugin/include/carma_guidance_plugin/tactical_plugin.hpp @@ -0,0 +1,70 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +namespace carma_guidance_plugin +{ + + /** + * \brief TacticalPlugin provides default functionality for all carma guidance plugins. + * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery + * + */ + class TacticalPlugin : public PluginBaseNode + { + + private: + // Services + carma_ros2_utils::ServicePtr plan_trajectory_service_; + + public: + /** + * \brief TacticalPlugin constructor + */ + explicit TacticalPlugin(const rclcpp::NodeOptions &); + + // TODO comments + virtual void plan_trajectory_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request &req, + carma_planning_msgs::srv::PlanTrajectory::Response &resp) = 0; + + + //// + // Overrides + //// + + // Non-Final + std::string get_capability() override; + + // Final + uint8_t get_type() override final; + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override final; + }; + +} // carma_guidance_plugin diff --git a/carma_guidance_plugin/launch/carma_guidance_plugin_launch.py b/carma_guidance_plugin/launch/carma_guidance_plugin_launch.py new file mode 100644 index 0000000000..1681a2ff52 --- /dev/null +++ b/carma_guidance_plugin/launch/carma_guidance_plugin_launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA carma_guidance_plugin_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('carma_guidance_plugin'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='carma_guidance_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='carma_guidance_plugin', + plugin='carma_guidance_plugin::PluginBaseNode', + name='carma_guidance_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/carma_guidance_plugin/package.xml b/carma_guidance_plugin/package.xml new file mode 100644 index 0000000000..383534e119 --- /dev/null +++ b/carma_guidance_plugin/package.xml @@ -0,0 +1,43 @@ + + + + + + carma_guidance_plugin + 1.0.0 + The carma_guidance_plugin package + + carma + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + std_srvs + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + + diff --git a/carma_guidance_plugin/src/control_plugin.cpp b/carma_guidance_plugin/src/control_plugin.cpp new file mode 100644 index 0000000000..d9b5bfada9 --- /dev/null +++ b/carma_guidance_plugin/src/control_plugin.cpp @@ -0,0 +1,93 @@ +/* + * 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. + */ + +#include "carma_guidance_plugin/tactical_plugin.hpp" + +namespace carma_guidance_plugin +{ + namespace std_ph = std::placeholders; + + ControlPlugin::ControlPlugin(const rclcpp::NodeOptions &options) + : carma_ros2_utils::PluginBaseNode(options) + {} + + std::string ControlPlugin::get_capability() + { + return "control/trajectory_control"; + } + + uint8_t ControlPlugin::get_type() + { + return carma_planning_msgs::msg::Plugin::CONTROL; + } + + void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg) + { + current_pose_ = *msg; + } + + void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg) + { + current_twist_ = *msg; + } + + void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg) + { + current_trajectory_ = *msg; + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + // Initialize plan maneuvers service + current_pose_sub_ = create_subscription("current_pose", + std::bind(&ControlPlugin::current_pose_callback, this, std_ph::_1)); + + current_velocity_sub_ = create_subscription("vehicle/twist", + std::bind(&ControlPlugin::current_velocity_callback, this, std_ph::_1)); + + trajectory_plan_sub_ = create_subscription(get_name() + "/plan_trajectory", + std::bind(&ControlPlugin::current_trajectory_callback, this, std_ph::_1)); + + command_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(33), // Spin at 30 Hz per plugin API + std::bind(&ControlPlugin::generate_command, this)); + + return PluginBaseNode::handle_on_configure(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_activate(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_deactivate(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_shutdown(prev_state); + } + + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_error(prev_state); + } + +} // carma_guidance_plugin + diff --git a/carma_guidance_plugin/src/main.cpp b/carma_guidance_plugin/src/main.cpp new file mode 100644 index 0000000000..cb22340e7e --- /dev/null +++ b/carma_guidance_plugin/src/main.cpp @@ -0,0 +1,33 @@ +/* + * 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. + */ + +#include +#include "carma_guidance_plugin/carma_guidance_plugin_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/carma_guidance_plugin/src/plugin_base_node.cpp b/carma_guidance_plugin/src/plugin_base_node.cpp new file mode 100644 index 0000000000..39b41eb4b7 --- /dev/null +++ b/carma_guidance_plugin/src/plugin_base_node.cpp @@ -0,0 +1,93 @@ +/* + * 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. + */ +#include + +#include "carma_guidance_plugin/plugin_base_node.hpp" + +namespace carma_guidance_plugin +{ + namespace std_ph = std::placeholders; + + PluginBaseNode::PluginBaseNode(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) + { + // Setup plugin publisher discovery which should bypass lifecycle behavior to ensure plugins are found + // prior to them needing to be activated. + // NOTE: Any other topics which need to be setup in the future should use handle_on_configure and the default this->create_publisher method to get a lifecycle publisher instead + plugin_discovery_pub_ = rclcpp::create_publisher(this, "plugin_discovery",1); + + // Setup discovery timer to publish onto the plugin_discovery_pub + discovert_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(1000), // TODO double check frequency of update. Seems like it should be at least 500ms to hit nyquist frequency for arbitration calls + std::bind(&PluginBaseNode::discovery_timer_callback, this)); + } + + bool PluginBaseNode::get_activation_status() { + // Determine the plugin activation state by checking which lifecycle state we are in. + // If we are active then the plugin is active otherwise the plugin is inactive + return get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_configure(const rclcpp_lifecycle::State &) + { + return configure_plugin(); + } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_activate(const rclcpp_lifecycle::State &) + { + return activate_plugin(); + } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_deactivate(const rclcpp_lifecycle::State &) + { + return deactivate_plugin(); + } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_shutdown(const rclcpp_lifecycle::State &) + { + if (get_activation_status()) + deactivate_plugin(); // Try to deactivate first on shutdown + + return CallbackReturn::SUCCESS; + } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_error(const rclcpp_lifecycle::State &) + { + if (get_activation_status()) + deactivate_plugin(); // Try to deactivate first on error + + return CallbackReturn::SUCCESS; + } + + uint8_t PluginBaseNode::get_type() + { + // Base class returns unknown. + // Its expected that 2nd level extending classes (strategic, tactical, control) will return correct type by overriding + return carma_planning_msgs::msg::Plugin::UNKNOWN; + } + + void PluginBaseNode::discovery_timer_callback() + { + carma_planning_msgs::msg::Plugin msg; + msg.name = get_name(); + msg.version_id = get_version_id(); + msg.type = get_type(); + msg.available = get_availability(); + msg.activated = get_activation_status(); + msg.capability = get_capability(); + + plugin_discovery_pub_->publish(msg); + } + +} // carma_guidance_plugin + diff --git a/carma_guidance_plugin/src/strategic_plugin.cpp b/carma_guidance_plugin/src/strategic_plugin.cpp new file mode 100644 index 0000000000..681e126553 --- /dev/null +++ b/carma_guidance_plugin/src/strategic_plugin.cpp @@ -0,0 +1,67 @@ +/* + * 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. + */ + +#include "carma_guidance_plugin/strategic_plugin.hpp" + +namespace carma_guidance_plugin +{ + namespace std_ph = std::placeholders; + + StrategicPlugin::StrategicPlugin(const rclcpp::NodeOptions &options) + : carma_ros2_utils::PluginBaseNode(options) + {} + + std::string StrategicPlugin::get_capability() + { + return "strategic_plan/plan_maneuvers"; + } + + uint8_t StrategicPlugin::get_type() + { + return carma_planning_msgs::msg::Plugin::STRATEGIC; + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + // Initialize plan maneuvers service + plan_maneuvers_service_ = create_service(get_name() + "/plan_maneuvers", + std::bind(&StrategicPlugin::plan_maneuvers_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); + + return PluginBaseNode::handle_on_configure(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_activate(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_deactivate(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_shutdown(prev_state); + } + + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_error(prev_state); + } + +} // carma_guidance_plugin + diff --git a/carma_guidance_plugin/src/tactical_plugin.cpp b/carma_guidance_plugin/src/tactical_plugin.cpp new file mode 100644 index 0000000000..d641fcfdd4 --- /dev/null +++ b/carma_guidance_plugin/src/tactical_plugin.cpp @@ -0,0 +1,67 @@ +/* + * 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. + */ + +#include "carma_guidance_plugin/tactical_plugin.hpp" + +namespace carma_guidance_plugin +{ + namespace std_ph = std::placeholders; + + TacticalPlugin::TacticalPlugin(const rclcpp::NodeOptions &options) + : carma_ros2_utils::PluginBaseNode(options) + {} + + std::string TacticalPlugin::get_capability() + { + return "tactical_plan/plan_trajectory"; + } + + uint8_t TacticalPlugin::get_type() + { + return carma_planning_msgs::msg::Plugin::TACTICAL; + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) + { + // Initialize plan trajectory service + plan_trajectory_service_ = create_service(get_name() + "/plan_trajectory", + std::bind(&TacticalPlugin::plan_trajectory_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); + + return PluginBaseNode::handle_on_configure(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_activate(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_deactivate(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_shutdown(prev_state); + } + + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_error(prev_state); + } + +} // carma_guidance_plugin + diff --git a/carma_guidance_plugin/test/node_test.cpp b/carma_guidance_plugin/test/node_test.cpp new file mode 100644 index 0000000000..f83fd0d57e --- /dev/null +++ b/carma_guidance_plugin/test/node_test.cpp @@ -0,0 +1,55 @@ +/* + * 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. + */ + +#include +#include +#include +#include +#include + +#include "carma_guidance_plugin/carma_guidance_plugin_node.hpp" + + +// TODO for USER: Implement a real test using GTest +TEST(Testcarma_guidance_plugin, example_test){ + + rclcpp::NodeOptions options; + auto worker_node = std::make_shared(options); + + worker_node->configure(); //Call configure state transition + worker_node->activate(); //Call activate state transition to get not read for runtime + + std::unique_ptr msg = std::make_unique(); + msg->data = "my string"; + + worker_node->example_callback(move(msg)); // Manually drive topic callbacks + +} + +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 From 289b562d125155e16fb5630826c5efe796899af0 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Mon, 23 May 2022 14:27:39 -0700 Subject: [PATCH 004/165] WIP plugin controller --- .../config/guidance_controller_config.yaml | 2 +- .../guidance_controller/entry.h | 35 ++++ .../guidance_controller/entry_manager.h | 73 ++++++++ .../guidance_controller.hpp | 28 ++- .../guidance_controller}/plugin_manager.h | 20 +-- .../src/guidance_controller/entry.cpp | 24 +++ .../src/guidance_controller/entry_manager.cpp | 87 ++++++++++ .../guidance_controller.cpp | 71 ++++++++ .../guidance_controller}/plugin_manager.cpp | 20 +-- .../test/test_entry_manager.cpp | 162 ++++++++++++++++++ .../test/test_plugin_manager.cpp | 101 +++++++++++ 11 files changed, 600 insertions(+), 23 deletions(-) create mode 100644 subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h create mode 100644 subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h rename {health_monitor/include => subsystem_controllers/include/subsystem_controllers/guidance_controller}/plugin_manager.h (72%) create mode 100644 subsystem_controllers/src/guidance_controller/entry.cpp create mode 100644 subsystem_controllers/src/guidance_controller/entry_manager.cpp rename {health_monitor/src => subsystem_controllers/src/guidance_controller}/plugin_manager.cpp (82%) create mode 100644 subsystem_controllers/test/test_entry_manager.cpp create mode 100644 subsystem_controllers/test/test_plugin_manager.cpp diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 224906401f..1937bdd577 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -9,7 +9,7 @@ call_timeout_ms : 1000 # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller - subsystem_namespace: /guidance + subsystem_namespace: /guidance # TODO we need to decouple this from the plugins # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed required_subsystem_nodes: diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h new file mode 100644 index 0000000000..09516fb40e --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h @@ -0,0 +1,35 @@ +#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 + +namespace subsystem_controllers +{ + struct Entry + { + /* data */ + bool available_; + bool active_; + std::string name_; + 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); + }; +} \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h new file mode 100644 index 0000000000..6ddd5fd6d9 --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h @@ -0,0 +1,73 @@ +#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 "entry.h" + +namespace subsystem_controllers +{ + class EntryManager + { + public: + + /*! + * \brief Default constructor for EntryManager. + */ + EntryManager(); + /*! + * \brief Constructor for EntryManager to set required entries. + */ + EntryManager(std::vector required_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; + + private: + + // private list to keep track of all entries + std::vector entry_list_; + + // list of required entries + std::vector required_entries_; + + }; +} \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp index 7eb9934f5b..3e0d331180 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp @@ -27,6 +27,7 @@ namespace subsystem_controllers { + using cr2 = carma_ros2_utils class GuidanceControllerNode : public BaseSubsystemController { @@ -43,8 +44,31 @@ namespace subsystem_controllers */ explicit GuidanceControllerNode(const rclcpp::NodeOptions &options); - // TODO The ROS1 plugin manager functionality should be updated to properly interact with or exist in this node or a guidance_plugin_controller node - // https://github.com/usdot-fhwa-stol/carma-platform/issues/1499 + cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &); + + cr2::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + + cr2::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &); + + private: + //! Plugin manager to handle all the plugin specific discovery and reporting + std::shared_ptr plugin_manager_; + + //! ROS handles + + cr2::SubPtr plugin_discovery_sub_; + + cr2::ServicePtr get_registered_plugins_server_; + + cr2::ServicePtr get_active_plugins_server_; + + cr2::ServicePtr activate_plugin_server_; + + cr2::ServicePtr get_strategic_plugin_by_capability_server_; + + cr2::ServicePtrget_tactical_plugin_by_capability_server_; + + std::unordered_set auto_activated_plugins_; }; diff --git a/health_monitor/include/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h similarity index 72% rename from health_monitor/include/plugin_manager.h rename to subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index bbd852d216..6e854f168a 100644 --- a/health_monitor/include/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -16,16 +16,16 @@ * the License. */ -#include -#include -#include -#include +#include +#include +#include +#include #include "entry_manager.h" #include #include -namespace health_monitor +namespace subsystem_controllers { class PluginManager { @@ -47,12 +47,12 @@ namespace health_monitor /** * \brief Get a list of registered plugins */ - void get_registered_plugins(cav_srvs::PluginListResponse& res); + void get_registered_plugins(carma_planning_msgs::PluginListResponse& res); /** * \brief Get a list of active plugins */ - void get_active_plugins(cav_srvs::PluginListResponse& res); + void get_active_plugins(carma_planning_msgs::PluginListResponse& res); /** * \brief Activate or deactivate a certain plugin @@ -62,17 +62,17 @@ namespace health_monitor /** * \brief Update the status of a certain plugin */ - void update_plugin_status(const cav_msgs::PluginConstPtr& msg); + void update_plugin_status(const carma_planning_msgs::PluginConstPtr& msg); /** * \brief Get strategic plugins by capability */ - bool get_strategic_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res); + bool get_strategic_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); /** * \brief Get tactical plugins by capability */ - bool get_tactical_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res); + bool get_tactical_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); private: diff --git a/subsystem_controllers/src/guidance_controller/entry.cpp b/subsystem_controllers/src/guidance_controller/entry.cpp new file mode 100644 index 0000000000..dce75170af --- /dev/null +++ b/subsystem_controllers/src/guidance_controller/entry.cpp @@ -0,0 +1,24 @@ +/* + * 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 subsystem_controllers +{ + 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/subsystem_controllers/src/guidance_controller/entry_manager.cpp b/subsystem_controllers/src/guidance_controller/entry_manager.cpp new file mode 100644 index 0000000000..2257f3731f --- /dev/null +++ b/subsystem_controllers/src/guidance_controller/entry_manager.cpp @@ -0,0 +1,87 @@ +/* + * 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_manager.h" +#include + +namespace subsystem_controllers +{ + + EntryManager::EntryManager() {} + + EntryManager::EntryManager(std::vector required_entries):required_entries_(required_entries) {} + + void EntryManager::update_entry(const Entry& entry) + { + for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) + { + if(i->name_.compare(entry.name_) == 0) + { + // name and type of the entry wont change + i->active_ = entry.active_; + i->available_ = entry.available_; + i->timestamp_ = entry.timestamp_; + return; + } + } + entry_list_.push_back(entry); + } + + + std::vector EntryManager::get_entries() const + { + // returns the copy of the original list + return std::vector(entry_list_); + } + + void EntryManager::delete_entry(const std::string& name) + { + for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) + { + if(i->name_.compare(name) == 0) + { + entry_list_.erase(i); + return; + } + } + } + + boost::optional EntryManager::get_entry_by_name(const std::string& name) const + { + for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) + { + if(i->name_.compare(name) == 0) + { + return *i; + } + } + // use boost::optional because requested entry might not exist + return boost::none; + } + + bool EntryManager::is_entry_required(const std::string& name) const + { + for(auto i = required_entries_.begin(); i < required_entries_.end(); ++i) + { + if(i->compare(name) == 0) + { + return true; + } + } + return false; + } + +} diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 980173701e..919b63e028 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -24,6 +24,77 @@ namespace subsystem_controllers { } + cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &) { + auto base_return = BaseSubsystemController::handle_on_configure(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not configure"); + return base_return; + } + + // TODO load parameters + + plugin_manager_ = std::make_shared(required_plugins_, plugin_service_prefix_, strategic_plugin_service_suffix_, tactical_plugin_service_suffix_); + + plugin_discovery_sub_ = create_subscription( + "plugin_discovery", 50, + std::bind(&PluginManager::update_plugin_status, plugin_manager_, std::placeholders::_1)); + + get_registered_plugins_server_ = create_service( + "plugins/get_registered_plugins", + std::bind(&PluginManager::get_registered_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + + get_active_plugins_server_ = create_service( + "plugins/get_active_plugins", + std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + + activate_plugin_server_ = create_service( + "plugins/get_active_plugins", + std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + + get_strategic_plugin_by_capability_server_ = create_service( + "plugins/get_strategic_plugin_by_capability", + std::bind(&PluginManager::get_strategic_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + + get_tactical_plugin_by_capability_server_ = create_service( + "plugins/get_tactical_plugin_by_capability", + std::bind(&PluginManager::get_tactical_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + + + return cr2::CallbackReturn::SUCCESS; + } + + cr2::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) + { + auto base_return = BaseSubsystemController::handle_on_activate(prev_state); // This will activate all nodes in the namespace TODO what about the plugins? + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not activate"); + return base_return; + } + + // In activate we want to activate all plugins which are required if they haven't already been activated by base class + // We want to skip all plugins which are not required + // TODO it seems like we may want some sort of start activated configuration for plugins as well + + plugin_manager_-> + + } + + cr2::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) + { + auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not deactivate"); + return base_return; + } + + // Iterate over all plugins to deactivate them + plugin_manager_.get_registered_plugins(); + } + + } // namespace subsystem_controllers #include "rclcpp_components/register_node_macro.hpp" diff --git a/health_monitor/src/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp similarity index 82% rename from health_monitor/src/plugin_manager.cpp rename to subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 5b50b9f228..72945666bb 100644 --- a/health_monitor/src/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -16,7 +16,7 @@ #include "plugin_manager.h" -namespace health_monitor +namespace subsystem_controllers { PluginManager::PluginManager(const std::vector& require_plugin_names, @@ -29,13 +29,13 @@ namespace health_monitor em_(EntryManager(require_plugin_names)) {} - void PluginManager::get_registered_plugins(cav_srvs::PluginListResponse& res) + void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginListResponse& res) { std::vector plugins = em_.get_entries(); // convert to plugin list for(auto i = plugins.begin(); i < plugins.end(); ++i) { - cav_msgs::Plugin plugin; + carma_planning_msgs::msg::Plugin plugin; plugin.activated = i->active_; plugin.available = i->available_; plugin.name = i->name_; @@ -44,7 +44,7 @@ namespace health_monitor } } - void PluginManager::get_active_plugins(cav_srvs::PluginListResponse& res) + void PluginManager::get_active_plugins(carma_planning_msgs::srv::PluginListResponse& res) { std::vector plugins = em_.get_entries(); // convert to plugin list @@ -52,7 +52,7 @@ namespace health_monitor { if(i->active_) { - cav_msgs::Plugin plugin; + carma_planning_msgs::msg::Plugin plugin; plugin.activated = true; plugin.available = i->available_; plugin.name = i->name_; @@ -75,7 +75,7 @@ namespace health_monitor return false; } - void PluginManager::update_plugin_status(const cav_msgs::PluginConstPtr& msg) + void PluginManager::update_plugin_status(const carma_planning_msgs::msg::PluginConstPtr& msg) { ROS_DEBUG_STREAM("received status from: " << msg->name); boost::optional requested_plugin = em_.get_entry_by_name(msg->name); @@ -92,11 +92,11 @@ namespace health_monitor em_.update_entry(plugin); } - bool PluginManager::get_tactical_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res) + bool PluginManager::get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) { for(const auto& plugin : em_.get_entries()) { - if(plugin.type_ == cav_msgs::Plugin::TACTICAL && + if(plugin.type_ == carma_planning_msgs::msg::Plugin::TACTICAL && (req.capability.size() == 0 || (plugin.capability_.compare(0, req.capability.size(), req.capability) == 0 && plugin.active_ && plugin.available_))) { res.plan_service.push_back(service_prefix_ + plugin.name_ + tactical_service_suffix_); @@ -105,11 +105,11 @@ namespace health_monitor return true; } - bool PluginManager::get_strategic_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res) + bool PluginManager::get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) { for(const auto& plugin : em_.get_entries()) { - if(plugin.type_ == cav_msgs::Plugin::STRATEGIC && + if(plugin.type_ == carma_planning_msgs::msg::Plugin::STRATEGIC && (req.capability.size() == 0 || (plugin.capability_.compare(0, req.capability.size(), req.capability) == 0 && plugin.active_ && plugin.available_))) { ROS_DEBUG_STREAM("discovered strategic plugin: " << plugin.name_); diff --git a/subsystem_controllers/test/test_entry_manager.cpp b/subsystem_controllers/test/test_entry_manager.cpp new file mode 100644 index 0000000000..b76f093572 --- /dev/null +++ b/subsystem_controllers/test/test_entry_manager.cpp @@ -0,0 +1,162 @@ +/* + * 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_manager.h" +#include +#include + +namespace subsystem_controllers +{ + + TEST(EntryManagerTest, testAddNewEntry) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin", 2000, 3, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + boost::optional res = em.get_entry_by_name("plugin"); + if(!res) + { + ASSERT_TRUE(1 == 1); + } else + { + ASSERT_TRUE(1 == 2); + } + } + + TEST(EntryManagerTest, testRequiredEntryCheck) + { + 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, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + 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")); + } + +} diff --git a/subsystem_controllers/test/test_plugin_manager.cpp b/subsystem_controllers/test/test_plugin_manager.cpp new file mode 100644 index 0000000000..fd481ef73e --- /dev/null +++ b/subsystem_controllers/test/test_plugin_manager.cpp @@ -0,0 +1,101 @@ +/* + * 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 "plugin_manager.h" +#include + +namespace subsystem_controllers +{ + TEST(PluginManagerTest, testRegisteredPlugins) + { + std::vector required_plugins{"autoware", "pure_pursuit"}; + PluginManager pm(required_plugins, "/guidance/plugin/", "/plan_maneuver", "/plan_trajectory"); + cav_msgs::Plugin msg1; + msg1.name = "autoware"; + msg1.available = true; + msg1.activated = false; + msg1.type = cav_msgs::Plugin::STRATEGIC; + msg1.version_id = "1.0.1"; + msg1.capability = "waypoint_following/autoware"; + cav_msgs::PluginConstPtr msg1_pointer(new cav_msgs::Plugin(msg1)); + pm.update_plugin_status(msg1_pointer); + cav_msgs::Plugin msg2; + msg2.name = "lane_change"; + msg2.available = true; + msg2.activated = true; + // this field is not used by plugin manager + msg2.type = cav_msgs::Plugin::TACTICAL; + msg2.version_id = "1.0.0"; + msg2.capability = "lane_change"; + cav_msgs::PluginConstPtr msg2_pointer(new cav_msgs::Plugin(msg2)); + pm.update_plugin_status(msg2_pointer); + cav_srvs::PluginListResponse res; + pm.get_registered_plugins(res); + EXPECT_EQ(2, res.plugins.size()); + // for cruising plugin + EXPECT_EQ(true, res.plugins.begin()->activated); + EXPECT_EQ(true, res.plugins.begin()->available); + EXPECT_EQ(0, res.plugins.begin()->name.compare("autoware")); + //EXPECT_EQ(0, res.plugins.begin()->type); + //EXPECT_EQ(0, res.plugins.begin()->version_id.compare("")); + // for lane change plugin + EXPECT_EQ(false, std::prev(res.plugins.end())->activated); + EXPECT_EQ(true, std::prev(res.plugins.end())->available); + EXPECT_EQ(0, std::prev(res.plugins.end())->name.compare("lane_change")); + //EXPECT_EQ(0, std::prev(res.plugins.end())->type); + //EXPECT_EQ(0, std::prev(res.plugins.end())->version_id.compare("")); + // TODO need to handle type and verionID + cav_srvs::PluginListResponse res2; + pm.get_active_plugins(res2); + EXPECT_EQ(true, res2.plugins.begin()->activated); + EXPECT_EQ(true, res2.plugins.begin()->available); + EXPECT_EQ(0, res2.plugins.begin()->name.compare("autoware")); + pm.activate_plugin("lane_change", true); + cav_srvs::PluginListResponse res3; + pm.get_active_plugins(res3); + EXPECT_EQ(2, res3.plugins.size()); + EXPECT_EQ(true, std::prev(res3.plugins.end())->activated); + EXPECT_EQ(true, std::prev(res3.plugins.end())->available); + EXPECT_EQ(0, std::prev(res3.plugins.end())->name.compare("lane_change")); + pm.activate_plugin("lane_change", false); + cav_srvs::PluginListResponse res4; + pm.get_active_plugins(res4); + EXPECT_EQ(1, res4.plugins.size()); + EXPECT_EQ(true, res4.plugins.begin()->activated); + EXPECT_EQ(true, res4.plugins.begin()->available); + EXPECT_EQ(0, res4.plugins.begin()->name.compare("autoware")); + cav_srvs::GetPluginApiRequest req; + req.capability = ""; + cav_srvs::GetPluginApiResponse resp; + EXPECT_TRUE(pm.get_strategic_plugins_by_capability(req, resp)); + EXPECT_EQ(1, resp.plan_service.size()); + EXPECT_EQ("/guidance/plugin/autoware/plan_maneuver", resp.plan_service[0]); + resp = cav_srvs::GetPluginApiResponse(); + EXPECT_TRUE(pm.get_tactical_plugins_by_capability(req, resp)); + EXPECT_EQ(1, resp.plan_service.size()); + EXPECT_EQ("/guidance/plugin/lane_change/plan_trajectory", resp.plan_service[0]); + resp = cav_srvs::GetPluginApiResponse(); + req.capability = "waypoint_following"; + EXPECT_TRUE(pm.get_strategic_plugins_by_capability(req, resp)); + EXPECT_EQ(1, resp.plan_service.size()); + EXPECT_EQ("/guidance/plugin/autoware/plan_maneuver", resp.plan_service[0]); + resp = cav_srvs::GetPluginApiResponse(); + req.capability = "platooning"; + EXPECT_TRUE(pm.get_strategic_plugins_by_capability(req, resp)); + EXPECT_EQ(0, resp.plan_service.size()); + } + +} From 3a91613938be57495fe1dbb7c1995371470119d8 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Tue, 24 May 2022 21:23:29 +0000 Subject: [PATCH 005/165] Completed first iteration on plugin base classes --- carma_guidance_plugin/README.md | 3 - carma_guidance_plugin/config/parameters.yaml | 3 - .../plugin_base_node.hpp | 83 -------- .../launch/carma_guidance_plugin_launch.py | 68 ------- carma_guidance_plugin/src/main.cpp | 33 ---- carma_guidance_plugin/test/node_test.cpp | 55 ------ .../CMakeLists.txt | 33 +--- carma_guidance_plugins/README.md | 3 + .../control_plugin.hpp | 53 +++-- .../plugin_base_node.hpp | 181 ++++++++++++++++++ .../strategic_plugin.hpp | 37 ++-- .../tactical_plugin.hpp | 37 ++-- .../package.xml | 11 +- .../src/control_plugin.cpp | 44 +++-- .../src/plugin_base_node.cpp | 77 ++++++-- .../src/strategic_plugin.cpp | 27 ++- .../src/tactical_plugin.cpp | 27 ++- carma_guidance_plugins/test/TestPlugins.h | 152 +++++++++++++++ carma_guidance_plugins/test/node_test.cpp | 162 ++++++++++++++++ 19 files changed, 720 insertions(+), 369 deletions(-) delete mode 100644 carma_guidance_plugin/README.md delete mode 100644 carma_guidance_plugin/config/parameters.yaml delete mode 100644 carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp delete mode 100644 carma_guidance_plugin/launch/carma_guidance_plugin_launch.py delete mode 100644 carma_guidance_plugin/src/main.cpp delete mode 100644 carma_guidance_plugin/test/node_test.cpp rename {carma_guidance_plugin => carma_guidance_plugins}/CMakeLists.txt (59%) create mode 100644 carma_guidance_plugins/README.md rename {carma_guidance_plugin/include/carma_guidance_plugin => carma_guidance_plugins/include/carma_guidance_plugins}/control_plugin.hpp (56%) create mode 100644 carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp rename {carma_guidance_plugin/include/carma_guidance_plugin => carma_guidance_plugins/include/carma_guidance_plugins}/strategic_plugin.hpp (50%) rename {carma_guidance_plugin/include/carma_guidance_plugin => carma_guidance_plugins/include/carma_guidance_plugins}/tactical_plugin.hpp (50%) rename {carma_guidance_plugin => carma_guidance_plugins}/package.xml (82%) rename {carma_guidance_plugin => carma_guidance_plugins}/src/control_plugin.cpp (63%) rename {carma_guidance_plugin => carma_guidance_plugins}/src/plugin_base_node.cpp (52%) rename {carma_guidance_plugin => carma_guidance_plugins}/src/strategic_plugin.cpp (70%) rename {carma_guidance_plugin => carma_guidance_plugins}/src/tactical_plugin.cpp (70%) create mode 100644 carma_guidance_plugins/test/TestPlugins.h create mode 100644 carma_guidance_plugins/test/node_test.cpp diff --git a/carma_guidance_plugin/README.md b/carma_guidance_plugin/README.md deleted file mode 100644 index e86518b063..0000000000 --- a/carma_guidance_plugin/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# carma_guidance_plugin - -TODO for USER: Add description of package and link to confluence documentation. diff --git a/carma_guidance_plugin/config/parameters.yaml b/carma_guidance_plugin/config/parameters.yaml deleted file mode 100644 index 5b061717c3..0000000000 --- a/carma_guidance_plugin/config/parameters.yaml +++ /dev/null @@ -1,3 +0,0 @@ -# String: Example Parameter -# TODO for USER: Add your own parameters -example_param : "My example value" diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp b/carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp deleted file mode 100644 index f79079a66d..0000000000 --- a/carma_guidance_plugin/include/carma_guidance_plugin/plugin_base_node.hpp +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Copyright (C) 2022 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * 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. - */ - -#pragma once - -#include -#include -#include - -#include - -namespace carma_guidance_plugin -{ - - /** - * \brief PluginBaseNode provides default functionality for all carma guidance plugins. - * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery - * - */ - class PluginBaseNode : public carma_ros2_utils::CarmaLifecycleNode - { - - private: - // Publishers - carma_ros2_utils::PubPtr plugin_discovery_pub_; - - // Timers - rclcpp::TimerBase::SharedPtr discovert_timer_; - - public: - /** - * \brief PluginBaseNode constructor - */ - explicit PluginBaseNode(const rclcpp::NodeOptions &); - - // TODO do I need the virtual destructor? - - /** - * \brief Callback for the plugin discovery timer which will publish the plugin discovery message - */ - void discovery_timer_callback(); - - // TODO comments - bool get_activation_status(); - - virtual uint8_t get_type(); - - virtual bool get_availability() = 0; - virtual std::string get_capability() = 0; - virtual std::string get_name() = 0; - virtual std::string get_version_id() = 0; - - virtual carma_ros2_utils::CallbackReturn configure_plugin() = 0; - virtual carma_ros2_utils::CallbackReturn activate_plugin() = 0; - virtual carma_ros2_utils::CallbackReturn deactivate_plugin() = 0; - - //// - // Overrides - //// - // For simplicity of managing the plugin state machine all lifecycle callbacks are implemented as final by the core extending classes (strategic, tactical, control) - // Plugins will use the simplified plugin callbacks via the template pattern - carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override; - carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override; - carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override; - carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override; - carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override; - - }; - -} // carma_guidance_plugin diff --git a/carma_guidance_plugin/launch/carma_guidance_plugin_launch.py b/carma_guidance_plugin/launch/carma_guidance_plugin_launch.py deleted file mode 100644 index 1681a2ff52..0000000000 --- a/carma_guidance_plugin/launch/carma_guidance_plugin_launch.py +++ /dev/null @@ -1,68 +0,0 @@ -# Copyright (C) 2022 LEIDOS. -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# 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_directory -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace - -import os - - -''' -This file is can be used to launch the CARMA carma_guidance_plugin_node. - Though in carma-platform it may be launched directly from the base launch file. -''' - -def generate_launch_description(): - - # Declare the log_level launch argument - log_level = LaunchConfiguration('log_level') - declare_log_level_arg = DeclareLaunchArgument( - name ='log_level', default_value='WARN') - - # Get parameter file path - param_file_path = os.path.join( - get_package_share_directory('carma_guidance_plugin'), 'config/parameters.yaml') - - - # Launch node(s) in a carma container to allow logging to be configured - container = ComposableNodeContainer( - package='carma_ros2_utils', - name='carma_guidance_plugin_container', - namespace=GetCurrentNamespace(), - executable='carma_component_container_mt', - composable_node_descriptions=[ - - # Launch the core node(s) - ComposableNode( - package='carma_guidance_plugin', - plugin='carma_guidance_plugin::PluginBaseNode', - name='carma_guidance_plugin_node', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : log_level } - ], - parameters=[ param_file_path ] - ), - ] - ) - - return LaunchDescription([ - declare_log_level_arg, - container - ]) diff --git a/carma_guidance_plugin/src/main.cpp b/carma_guidance_plugin/src/main.cpp deleted file mode 100644 index cb22340e7e..0000000000 --- a/carma_guidance_plugin/src/main.cpp +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (C) 2022 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * 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 "carma_guidance_plugin/carma_guidance_plugin_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/carma_guidance_plugin/test/node_test.cpp b/carma_guidance_plugin/test/node_test.cpp deleted file mode 100644 index f83fd0d57e..0000000000 --- a/carma_guidance_plugin/test/node_test.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright (C) 2022 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * 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 "carma_guidance_plugin/carma_guidance_plugin_node.hpp" - - -// TODO for USER: Implement a real test using GTest -TEST(Testcarma_guidance_plugin, example_test){ - - rclcpp::NodeOptions options; - auto worker_node = std::make_shared(options); - - worker_node->configure(); //Call configure state transition - worker_node->activate(); //Call activate state transition to get not read for runtime - - std::unique_ptr msg = std::make_unique(); - msg->data = "my string"; - - worker_node->example_callback(move(msg)); // Manually drive topic callbacks - -} - -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/carma_guidance_plugin/CMakeLists.txt b/carma_guidance_plugins/CMakeLists.txt similarity index 59% rename from carma_guidance_plugin/CMakeLists.txt rename to carma_guidance_plugins/CMakeLists.txt index e4b8cc18b8..339fea3a6f 100644 --- a/carma_guidance_plugin/CMakeLists.txt +++ b/carma_guidance_plugins/CMakeLists.txt @@ -14,7 +14,7 @@ # the License. cmake_minimum_required(VERSION 3.5) -project(carma_guidance_plugin) +project(carma_guidance_plugins) # Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) @@ -26,8 +26,7 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() # Name build targets -set(node_exec carma_guidance_plugin_node_exec) -set(node_lib carma_guidance_plugin_node) +set(node_lib carma_guidance_plugins_node) # Includes include_directories( @@ -36,20 +35,10 @@ include_directories( # Build ament_auto_add_library(${node_lib} SHARED - src/carma_guidance_plugin_node.cpp -) - -ament_auto_add_executable(${node_exec} - src/main.cpp -) - -# Register component -rclcpp_components_register_nodes(${node_lib} "carma_guidance_plugin::PluginBaseNode") - -# All locally created targets will need to be manually linked -# ament auto will handle linking of external dependencies -target_link_libraries(${node_exec} - ${node_lib} + src/plugin_base_node.cpp + src/strategic_plugin.cpp + src/tactical_plugin.cpp + src/control_plugin.cpp ) # Testing @@ -58,15 +47,13 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable - ament_add_gtest(test_carma_guidance_plugin test/node_test.cpp) + ament_add_gtest(test_carma_guidance_plugins test/node_test.cpp) - ament_target_dependencies(test_carma_guidance_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + ament_target_dependencies(test_carma_guidance_plugins ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) - target_link_libraries(test_carma_guidance_plugin ${node_lib}) + target_link_libraries(test_carma_guidance_plugins ${node_lib}) endif() # Install -ament_auto_package( - INSTALL_TO_SHARE config launch -) +ament_auto_package() diff --git a/carma_guidance_plugins/README.md b/carma_guidance_plugins/README.md new file mode 100644 index 0000000000..0739adbad7 --- /dev/null +++ b/carma_guidance_plugins/README.md @@ -0,0 +1,3 @@ +# carma_guidance_plugins + +This package provides a set of base classes to implement CARMA Platform Guidance Plugins API. You can read about plugins in the [CARMA Platform Architecture](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/89587713/CARMA+Platform+System+Architecture). The design of these base classes can be found [here](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2182545409/Detailed+Design+-+Plugin+Library). Using this library is not required as the plugin API is implemented entirely through ROS interfaces, however, using this package will minimize implementation errors. diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/control_plugin.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp similarity index 56% rename from carma_guidance_plugin/include/carma_guidance_plugin/control_plugin.hpp rename to carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp index 42252d9fd6..7bd8efd77a 100644 --- a/carma_guidance_plugin/include/carma_guidance_plugin/control_plugin.hpp +++ b/carma_guidance_plugins/include/carma_guidance_plugins/control_plugin.hpp @@ -17,17 +17,22 @@ #pragma once #include -#include -#include +#include +#include +#include +#include -#include +#include "carma_guidance_plugins/plugin_base_node.hpp" -namespace carma_guidance_plugin +namespace carma_guidance_plugins { /** - * \brief ControlPlugin provides default functionality for all carma guidance plugins. - * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery + * \brief ControlPlugin base class which can be extended by user provided plugins which wish to implement the Control Plugin ROS API. + * + * A control plugin is responsible for generating high frequency vehicle speed and steering commands to execute the currently planned trajectory. + * This plugin provides default subscribers to track the pose, velocity, and current trajectory in the system. + * Extending classes must implement the generate_command() method to use that data and or additional data to plan commands at a 30Hz frequency. * */ class ControlPlugin : public PluginBaseNode @@ -39,9 +44,17 @@ namespace carma_guidance_plugin carma_ros2_utils::SubPtr current_velocity_sub_; carma_ros2_utils::SubPtr trajectory_plan_sub_; + // Publishers + carma_ros2_utils::PubPtr vehicle_cmd_pub_; + // Timers rclcpp::TimerBase::SharedPtr command_timer_; + // These callbacks do direct assignment into their respective member variables + void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg); + void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg); + void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg); + protected: @@ -52,7 +65,7 @@ namespace carma_guidance_plugin // NOTE: Only the twist.linear.x and header are guaranteed to be populated boost::optional current_twist_; - // TODO comments, note user can determine if trajectory has changed based on trajectory_id + //! The most recent trajectory received by this plugin boost::optional current_trajectory_; @@ -62,19 +75,24 @@ namespace carma_guidance_plugin */ explicit ControlPlugin(const rclcpp::NodeOptions &); - // TODO comments - void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg); - void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg); - void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg); - - - virtual void generate_command() = 0; + //! Virtual destructor for safe deletion + virtual ~ControlPlugin() = default; + + /** + * \brief Extending class provided method which should generate a command message + * which will be published to the required topic by the base class + * + * NOTE: Implementer can determine if trajectory has changed based on current_trajectory_->trajectory_id + * + * \return The command message to publish + */ + virtual autoware_msgs::msg::ControlCommandStamped generate_command() = 0; //// // Overrides //// - // Non-Final + // Non-Final to allow extending plugins to provide more detailed capabilities std::string get_capability() override; // Final @@ -83,8 +101,9 @@ namespace carma_guidance_plugin carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; - carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final; }; -} // carma_guidance_plugin +} // carma_guidance_plugins diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp new file mode 100644 index 0000000000..9eb7f7183d --- /dev/null +++ b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp @@ -0,0 +1,181 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include + +#include + +namespace carma_guidance_plugins +{ + + /** + * \brief PluginBaseNode provides default functionality for all carma guidance plugins. + * This includes basic state machine management (largely delegated to ROS2 lifecycle behavior), required interfaces, and plugin discovery + * + * Extending classes must implement the on_configure_plugin method to load parameters, and my override the other state transitions methods on__plugin if desired. + * Additionally, extending classes must implement the methods such as get_name() which are used to populate the plugin discovery message. + * + */ + class PluginBaseNode : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Publishers + //! Publisher for plugin discovery. This publisher will begin publishing + //immediately on node construction regardless of lifecycle state. This is meant to allow for fast plugin activation on startup. + rclcpp::Publisher::SharedPtr plugin_discovery_pub_; + + // Timers + //! Timer to trigger publication of the plugin discovery message at a fixed frequency + rclcpp::TimerBase::SharedPtr discovert_timer_; + + /** + * \brief Callback for the plugin discovery timer which will publish the plugin discovery message + */ + void discovery_timer_callback(); + + public: + /** + * \brief PluginBaseNode constructor + */ + explicit PluginBaseNode(const rclcpp::NodeOptions &); + + //! Virtual destructor for safe deletion + virtual ~PluginBaseNode() = default; + + /** + * \brief Returns the activation status of this plugin. + * The plugins API callbacks will only be triggered when this method returns true. + * + * \return True if plugin is in the ACTIVE state. False otherwise. + */ + virtual bool get_activation_status() final; + + + /** + * \brief Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. + * Extending classes for the specific type should override this method. + * + * \return The extending class type or UNKOWN if the class or no parent class has implement this method. + */ + virtual uint8_t get_type(); + + /** + * \brief Get the availability status of this plugin based on the current operating environment. + * Method must be overriden by extending classes. + * + * \return This method should return true if the plugin's current understanding of the world + * means it would be capable of planning or executing its capability. + */ + virtual bool get_availability() = 0; + + /** + * \brief Get the capability string representing this plugins capabilities + * Method must be overriden by extending classes. + * Expectation is that abstract plugin type parent classes will provide a default implementation. + * + * \return Forward slash separated string describing the plugin's capabilities per the plugin capabilites API + */ + virtual std::string get_capability() = 0; + + /** + * \brief Returns the name of this plugin. + * This name may be automatically prepended to plugin API service or topic names. + * + * \return Name of this plugin + */ + virtual std::string get_name() = 0; + + /** + * \brief Returns the version id of this plugin. + * + * \return The version id represented as a string + */ + virtual std::string get_version_id() = 0; + + /** + * \brief Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. + * This method should be used to load parameters and is required to be implemented. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_configure_plugin() = 0; + + /** + * \brief Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. + * This method should be used to prepare for future callbacks for plugin's capabilites. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to ACTIVE will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_activate_plugin(); + + /** + * \brief Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. + * This method should be used to disable any functionality which should cease execution when plugin is inactive. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to INACTIVE will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_deactivate_plugin(); + + /** + * \brief Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. + * This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin + * in a fresh state as though just launched. + * + * \return SUCCESS, FAILURE, or ERROR. Transition to UNCONFIGURED will only complete on SUCCESS. + */ + virtual carma_ros2_utils::CallbackReturn on_cleanup_plugin(); + + /** + * \brief Method which is triggered when this plugin is moved from any state to FINALIZED + * This method should be used to generate any shutdown logs or final cleanup. + * + * \return SUCCESS, FAILURE, or ERROR. On ERROR, may temporarily go to on_error_plugin() before calling finalized. + */ + virtual carma_ros2_utils::CallbackReturn on_shutdown_plugin(); + + /** + * \brief Method which is triggered when an unhandled exception occurs in this plugin + * This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. + * + * \param exception_string The exception description + * + * \return SUCCESS, FAILURE, or ERROR. On SUCCESS plugin moves to UNCONFIGURED state else FINALIZED. Default behavior is to move to FINALIZED. + */ + virtual carma_ros2_utils::CallbackReturn on_error_plugin(const std::string &exception_string); + + //// + // Overrides + //// + // For simplicity of managing the plugin state machine all lifecycle callbacks are implemented as final by the core extending classes (strategic, tactical, control) + // Plugins will use the plugin specific callbacks via the template pattern + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override; + + // Unit Test Accessors + FRIEND_TEST(carma_guidance_plugins_test, connections_test); + + }; + +} // carma_guidance_plugins diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/strategic_plugin.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/strategic_plugin.hpp similarity index 50% rename from carma_guidance_plugin/include/carma_guidance_plugin/strategic_plugin.hpp rename to carma_guidance_plugins/include/carma_guidance_plugins/strategic_plugin.hpp index b830e129b4..1fd79f4753 100644 --- a/carma_guidance_plugin/include/carma_guidance_plugin/strategic_plugin.hpp +++ b/carma_guidance_plugins/include/carma_guidance_plugins/strategic_plugin.hpp @@ -17,17 +17,16 @@ #pragma once #include -#include -#include +#include -#include +#include "carma_guidance_plugins/plugin_base_node.hpp" -namespace carma_guidance_plugin +namespace carma_guidance_plugins { /** - * \brief StrategicPlugin provides default functionality for all carma guidance plugins. - * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery + * \brief StrategicPlugin base class which can be extended by user provided plugins which wish to implement the Strategic Plugin ROS API. + * A strategic plugin is responsible for planning sequences of maneuvers to achieve high-level objects for the vehicle (follow the route, efficiently get through an intersection, etc.) * */ class StrategicPlugin : public PluginBaseNode @@ -35,7 +34,9 @@ namespace carma_guidance_plugin private: // Services - carma_ros2_utils::ServicePtr plan_maneuvers_service_; + + //! The service which will be called when a strategic plugin needs to plan maneuvers + carma_ros2_utils::ServicePtr plan_maneuvers_service_; public: /** @@ -43,18 +44,27 @@ namespace carma_guidance_plugin */ explicit StrategicPlugin(const rclcpp::NodeOptions &); - // TODO comments + //! Virtual destructor for safe deletion + virtual ~StrategicPlugin() = default; + + /** + * \brief Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. + * + * \param srv_header RCL header for services calls. Can usually be ignored by implementers. + * \param req The service request containing the previously planned maneuvers and vehicle state + * \param resp The response containing the additional maneuvers generated by this plugin + */ virtual void plan_maneuvers_callback( std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanManeuvers::Request &req, - carma_planning_msgs::srv::PlanManeuvers::Response &resp) = 0; + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) = 0; //// // Overrides //// - // Non-Final + // Non-Final to allow extending plugins to provide more detailed capabilities std::string get_capability() override; // Final @@ -63,8 +73,9 @@ namespace carma_guidance_plugin carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; - carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final; }; -} // carma_guidance_plugin +} // carma_guidance_plugins diff --git a/carma_guidance_plugin/include/carma_guidance_plugin/tactical_plugin.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/tactical_plugin.hpp similarity index 50% rename from carma_guidance_plugin/include/carma_guidance_plugin/tactical_plugin.hpp rename to carma_guidance_plugins/include/carma_guidance_plugins/tactical_plugin.hpp index 5a8bc018f6..f3c9593960 100644 --- a/carma_guidance_plugin/include/carma_guidance_plugin/tactical_plugin.hpp +++ b/carma_guidance_plugins/include/carma_guidance_plugins/tactical_plugin.hpp @@ -17,17 +17,17 @@ #pragma once #include -#include -#include +#include -#include +#include "carma_guidance_plugins/plugin_base_node.hpp" -namespace carma_guidance_plugin +namespace carma_guidance_plugins { /** - * \brief TacticalPlugin provides default functionality for all carma guidance plugins. - * This includes basic state machine management (largely delegated to lifecycle behavior), required interfaces, and plugin discovery + * \brief TacticalPlugin base class which can be extended by user provided plugins which wish to implement the Tactical Plugin ROS API. + * + * A tactical plugin is responsible for planning the detailed trajectory which a vehicle should execute to complete a maneuver. * */ class TacticalPlugin : public PluginBaseNode @@ -35,7 +35,8 @@ namespace carma_guidance_plugin private: // Services - carma_ros2_utils::ServicePtr plan_trajectory_service_; + //! The ros service which can be called by the arbitrator or other plugins to have this plugin generate a trajectory plan + carma_ros2_utils::ServicePtr plan_trajectory_service_; public: /** @@ -43,18 +44,27 @@ namespace carma_guidance_plugin */ explicit TacticalPlugin(const rclcpp::NodeOptions &); - // TODO comments + //! Virtual destructor for safe deletion + virtual ~TacticalPlugin() = default; + + /** + * \brief Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. + * + * \param srv_header RCL header for services calls. Can usually be ignored by implementers. + * \param req The service request containing the maneuvers to plan trajectories for and current vehicle state + * \param resp The response containing the planned trajectory + */ virtual void plan_trajectory_callback( std::shared_ptr srv_header, - carma_planning_msgs::srv::PlanTrajectory::Request &req, - carma_planning_msgs::srv::PlanTrajectory::Response &resp) = 0; + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) = 0; //// // Overrides //// - // Non-Final + // Non-Final to allow extending plugins to provide more detailed capabilities std::string get_capability() override; // Final @@ -63,8 +73,9 @@ namespace carma_guidance_plugin carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final; carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final; - carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &) override final; + carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final; }; -} // carma_guidance_plugin +} // carma_guidance_plugins diff --git a/carma_guidance_plugin/package.xml b/carma_guidance_plugins/package.xml similarity index 82% rename from carma_guidance_plugin/package.xml rename to carma_guidance_plugins/package.xml index 383534e119..e69e405497 100644 --- a/carma_guidance_plugin/package.xml +++ b/carma_guidance_plugins/package.xml @@ -14,9 +14,9 @@ --> - carma_guidance_plugin + carma_guidance_plugins 1.0.0 - The carma_guidance_plugin package + This package provides a set of base classes to implement CARMA Platform Guidance Plugins API carma @@ -28,15 +28,12 @@ rclcpp carma_ros2_utils - rclcpp_components - std_srvs + carma_planning_msgs + autoware_msgs ament_lint_auto ament_cmake_gtest - launch - launch_ros - ament_cmake diff --git a/carma_guidance_plugin/src/control_plugin.cpp b/carma_guidance_plugins/src/control_plugin.cpp similarity index 63% rename from carma_guidance_plugin/src/control_plugin.cpp rename to carma_guidance_plugins/src/control_plugin.cpp index d9b5bfada9..b400a96d34 100644 --- a/carma_guidance_plugin/src/control_plugin.cpp +++ b/carma_guidance_plugins/src/control_plugin.cpp @@ -14,14 +14,16 @@ * the License. */ -#include "carma_guidance_plugin/tactical_plugin.hpp" +#include +#include "carma_guidance_plugins/control_plugin.hpp" -namespace carma_guidance_plugin + +namespace carma_guidance_plugins { namespace std_ph = std::placeholders; ControlPlugin::ControlPlugin(const rclcpp::NodeOptions &options) - : carma_ros2_utils::PluginBaseNode(options) + : PluginBaseNode(options) {} std::string ControlPlugin::get_capability() @@ -34,37 +36,44 @@ namespace carma_guidance_plugin return carma_planning_msgs::msg::Plugin::CONTROL; } - void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg) + void ControlPlugin::current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg) { current_pose_ = *msg; } - void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg) + void ControlPlugin::current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg) { current_twist_ = *msg; } - void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg) + void ControlPlugin::current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg) { current_trajectory_ = *msg; } carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { - // Initialize plan maneuvers service - current_pose_sub_ = create_subscription("current_pose", + // Initialize subscribers and publishers + current_pose_sub_ = create_subscription("current_pose", 1, std::bind(&ControlPlugin::current_pose_callback, this, std_ph::_1)); - current_velocity_sub_ = create_subscription("vehicle/twist", - std::bind(&ControlPlugin::current_velocity_callback, this, std_ph::_1)); + current_velocity_sub_ = create_subscription("vehicle/twist", 1, + std::bind(&ControlPlugin::current_twist_callback, this, std_ph::_1)); - trajectory_plan_sub_ = create_subscription(get_name() + "/plan_trajectory", + trajectory_plan_sub_ = create_subscription(get_name() + "/plan_trajectory", 1, std::bind(&ControlPlugin::current_trajectory_callback, this, std_ph::_1)); + vehicle_cmd_pub_ = create_publisher("ctrl_raw", 1); + command_timer_ = create_timer( get_clock(), std::chrono::milliseconds(33), // Spin at 30 Hz per plugin API - std::bind(&ControlPlugin::generate_command, this)); + [this]() { + if (this->get_activation_status()) // Only trigger when activated + { + this->vehicle_cmd_pub_->publish(this->generate_command()); + } + }); return PluginBaseNode::handle_on_configure(prev_state); } @@ -79,15 +88,20 @@ namespace carma_guidance_plugin return PluginBaseNode::handle_on_deactivate(prev_state); } + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_cleanup(prev_state); + } + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) { return PluginBaseNode::handle_on_shutdown(prev_state); } - carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state) + carma_ros2_utils::CallbackReturn ControlPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state, const std::string &exception_string) { - return PluginBaseNode::handle_on_error(prev_state); + return PluginBaseNode::handle_on_error(prev_state, exception_string); } -} // carma_guidance_plugin +} // carma_guidance_plugins diff --git a/carma_guidance_plugin/src/plugin_base_node.cpp b/carma_guidance_plugins/src/plugin_base_node.cpp similarity index 52% rename from carma_guidance_plugin/src/plugin_base_node.cpp rename to carma_guidance_plugins/src/plugin_base_node.cpp index 39b41eb4b7..00e87c9019 100644 --- a/carma_guidance_plugin/src/plugin_base_node.cpp +++ b/carma_guidance_plugins/src/plugin_base_node.cpp @@ -13,26 +13,23 @@ * License for the specific language governing permissions and limitations under * the License. */ +#include #include -#include "carma_guidance_plugin/plugin_base_node.hpp" +#include "carma_guidance_plugins/plugin_base_node.hpp" -namespace carma_guidance_plugin +namespace carma_guidance_plugins { namespace std_ph = std::placeholders; PluginBaseNode::PluginBaseNode(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options) { - // Setup plugin publisher discovery which should bypass lifecycle behavior to ensure plugins are found - // prior to them needing to be activated. - // NOTE: Any other topics which need to be setup in the future should use handle_on_configure and the default this->create_publisher method to get a lifecycle publisher instead - plugin_discovery_pub_ = rclcpp::create_publisher(this, "plugin_discovery",1); // Setup discovery timer to publish onto the plugin_discovery_pub discovert_timer_ = create_timer( get_clock(), - std::chrono::milliseconds(1000), // TODO double check frequency of update. Seems like it should be at least 500ms to hit nyquist frequency for arbitration calls + std::chrono::milliseconds(500), // 2 Hz frequency to account for 1Hz maneuver planning frequency std::bind(&PluginBaseNode::discovery_timer_callback, this)); } @@ -42,31 +39,60 @@ namespace carma_guidance_plugin return get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; } + carma_ros2_utils::CallbackReturn PluginBaseNode::on_activate_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_deactivate_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_cleanup_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_shutdown_plugin() + { + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::on_error_plugin(const std::string &) + { + // On error should default to failure so user must explicitly implement error handling to get any other behavior + return carma_ros2_utils::CallbackReturn::FAILURE; + } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_configure(const rclcpp_lifecycle::State &) { - return configure_plugin(); + return on_configure_plugin(); } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_activate(const rclcpp_lifecycle::State &) { - return activate_plugin(); + return on_activate_plugin(); } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_deactivate(const rclcpp_lifecycle::State &) { - return deactivate_plugin(); + return on_deactivate_plugin(); + } + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_cleanup(const rclcpp_lifecycle::State &) + { + return on_cleanup_plugin(); } + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_shutdown(const rclcpp_lifecycle::State &) { - if (get_activation_status()) - deactivate_plugin(); // Try to deactivate first on shutdown - - return CallbackReturn::SUCCESS; + return on_shutdown_plugin(); } - carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_error(const rclcpp_lifecycle::State &) + + carma_ros2_utils::CallbackReturn PluginBaseNode::handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) { - if (get_activation_status()) - deactivate_plugin(); // Try to deactivate first on error - - return CallbackReturn::SUCCESS; + return on_error_plugin(exception_string); } uint8_t PluginBaseNode::get_type() @@ -78,6 +104,17 @@ namespace carma_guidance_plugin void PluginBaseNode::discovery_timer_callback() { + + if (!plugin_discovery_pub_) // If the publisher has not been initalized then initialize it. + { + // Setup plugin publisher discovery which should bypass lifecycle behavior to ensure plugins are found + // prior to them needing to be activated. + // NOTE: Any other topics which need to be setup in the future should use handle_on_configure and the default this->create_publisher method to get a lifecycle publisher instead + auto this_shared = shared_from_this(); // Usage of shared_from_this() means this cannot be done in the constructor thus it is delegated to the timer callback + plugin_discovery_pub_ = rclcpp::create_publisher(this_shared, "plugin_discovery", 1); + + } + carma_planning_msgs::msg::Plugin msg; msg.name = get_name(); msg.version_id = get_version_id(); @@ -89,5 +126,5 @@ namespace carma_guidance_plugin plugin_discovery_pub_->publish(msg); } -} // carma_guidance_plugin +} // carma_guidance_plugins diff --git a/carma_guidance_plugin/src/strategic_plugin.cpp b/carma_guidance_plugins/src/strategic_plugin.cpp similarity index 70% rename from carma_guidance_plugin/src/strategic_plugin.cpp rename to carma_guidance_plugins/src/strategic_plugin.cpp index 681e126553..5209c46e34 100644 --- a/carma_guidance_plugin/src/strategic_plugin.cpp +++ b/carma_guidance_plugins/src/strategic_plugin.cpp @@ -14,14 +14,15 @@ * the License. */ -#include "carma_guidance_plugin/strategic_plugin.hpp" +#include +#include "carma_guidance_plugins/strategic_plugin.hpp" -namespace carma_guidance_plugin +namespace carma_guidance_plugins { namespace std_ph = std::placeholders; StrategicPlugin::StrategicPlugin(const rclcpp::NodeOptions &options) - : carma_ros2_utils::PluginBaseNode(options) + : PluginBaseNode(options) {} std::string StrategicPlugin::get_capability() @@ -37,8 +38,13 @@ namespace carma_guidance_plugin carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { // Initialize plan maneuvers service - plan_maneuvers_service_ = create_service(get_name() + "/plan_maneuvers", - std::bind(&StrategicPlugin::plan_maneuvers_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); + plan_maneuvers_service_ = create_service(get_name() + "/plan_maneuvers", + [this] (auto header, auto req, auto resp) { + if (this->get_activation_status()) // Only trigger when activated + { + this->plan_maneuvers_callback(header, req, resp); + } + }); return PluginBaseNode::handle_on_configure(prev_state); } @@ -53,15 +59,20 @@ namespace carma_guidance_plugin return PluginBaseNode::handle_on_deactivate(prev_state); } + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_cleanup(prev_state); + } + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) { return PluginBaseNode::handle_on_shutdown(prev_state); } - carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state) + carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state, const std::string &exception_string) { - return PluginBaseNode::handle_on_error(prev_state); + return PluginBaseNode::handle_on_error(prev_state, exception_string); } -} // carma_guidance_plugin +} // carma_guidance_plugins diff --git a/carma_guidance_plugin/src/tactical_plugin.cpp b/carma_guidance_plugins/src/tactical_plugin.cpp similarity index 70% rename from carma_guidance_plugin/src/tactical_plugin.cpp rename to carma_guidance_plugins/src/tactical_plugin.cpp index d641fcfdd4..345acfd9a8 100644 --- a/carma_guidance_plugin/src/tactical_plugin.cpp +++ b/carma_guidance_plugins/src/tactical_plugin.cpp @@ -14,14 +14,15 @@ * the License. */ -#include "carma_guidance_plugin/tactical_plugin.hpp" +#include +#include "carma_guidance_plugins/tactical_plugin.hpp" -namespace carma_guidance_plugin +namespace carma_guidance_plugins { namespace std_ph = std::placeholders; TacticalPlugin::TacticalPlugin(const rclcpp::NodeOptions &options) - : carma_ros2_utils::PluginBaseNode(options) + : PluginBaseNode(options) {} std::string TacticalPlugin::get_capability() @@ -37,8 +38,13 @@ namespace carma_guidance_plugin carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { // Initialize plan trajectory service - plan_trajectory_service_ = create_service(get_name() + "/plan_trajectory", - std::bind(&TacticalPlugin::plan_trajectory_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); + plan_trajectory_service_ = create_service(get_name() + "/plan_trajectory", + [this] (auto header, auto req, auto resp) { + if (this->get_activation_status()) // Only trigger when activated + { + this->plan_trajectory_callback(header, req, resp); + } + }); return PluginBaseNode::handle_on_configure(prev_state); } @@ -53,15 +59,20 @@ namespace carma_guidance_plugin return PluginBaseNode::handle_on_deactivate(prev_state); } + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) + { + return PluginBaseNode::handle_on_cleanup(prev_state); + } + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) { return PluginBaseNode::handle_on_shutdown(prev_state); } - carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state) + carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_error(const rclcpp_lifecycle::State &prev_state, const std::string &exception_string) { - return PluginBaseNode::handle_on_error(prev_state); + return PluginBaseNode::handle_on_error(prev_state, exception_string); } -} // carma_guidance_plugin +} // carma_guidance_plugins diff --git a/carma_guidance_plugins/test/TestPlugins.h b/carma_guidance_plugins/test/TestPlugins.h new file mode 100644 index 0000000000..567c925fba --- /dev/null +++ b/carma_guidance_plugins/test/TestPlugins.h @@ -0,0 +1,152 @@ +/* + * 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. + */ + +#pragma once + +#include "carma_guidance_plugins/strategic_plugin.hpp" +#include "carma_guidance_plugins/tactical_plugin.hpp" +#include "carma_guidance_plugins/control_plugin.hpp" + +namespace carma_guidance_plugins +{ + class TestStrategicPlugin : public StrategicPlugin + { + public: + + explicit TestStrategicPlugin(const rclcpp::NodeOptions &options) + : StrategicPlugin(options) + {} + + ~TestStrategicPlugin() = default; + + + void plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override + { + + } + + bool get_availability() override { + return true; + } + + std::string get_name() override { + return "TestStrategicPlugin"; + } + + std::string get_version_id() override { + return "1.0"; + } + + carma_ros2_utils::CallbackReturn on_configure_plugin() override + { + RCLCPP_INFO(get_logger(), "Configuring TestStrategicPlugin"); + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + std::string get_capability() override { + return StrategicPlugin::get_capability() + "/test_capability"; + }; + + }; + + class TestTacticalPlugin : public TacticalPlugin + { + public: + + explicit TestTacticalPlugin(const rclcpp::NodeOptions &options) + : TacticalPlugin(options) + {} + + ~TestTacticalPlugin() = default; + + + void plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr) override + { + + } + + bool get_availability() override { + return true; + } + + std::string get_name() override { + return "TestTacticalPlugin"; + } + + std::string get_version_id() override { + return "1.1"; + } + + carma_ros2_utils::CallbackReturn on_configure_plugin() override + { + RCLCPP_INFO(get_logger(), "Configuring TestTacticalPlugin"); + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + std::string get_capability() override { + return TacticalPlugin::get_capability() + "/test_capability"; + }; + + }; + + class TestControlPlugin : public ControlPlugin + { + public: + + explicit TestControlPlugin(const rclcpp::NodeOptions &options) + : ControlPlugin(options) + {} + + ~TestControlPlugin() = default; + + + autoware_msgs::msg::ControlCommandStamped generate_command() override + { + autoware_msgs::msg::ControlCommandStamped msg; + return msg; + } + + bool get_availability() override { + return true; + } + + std::string get_name() override { + return "TestControlPlugin"; + } + + std::string get_version_id() override { + return "1.2"; + } + + carma_ros2_utils::CallbackReturn on_configure_plugin() override + { + RCLCPP_INFO(get_logger(), "Configuring TestControlPlugin"); + + return carma_ros2_utils::CallbackReturn::SUCCESS; + } + + std::string get_capability() override { + return ControlPlugin::get_capability() + "/test_capability"; + }; + + }; +} \ No newline at end of file diff --git a/carma_guidance_plugins/test/node_test.cpp b/carma_guidance_plugins/test/node_test.cpp new file mode 100644 index 0000000000..cf36f5015a --- /dev/null +++ b/carma_guidance_plugins/test/node_test.cpp @@ -0,0 +1,162 @@ +/* + * 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. + */ + +#include +#include +#include +#include +#include + +#include "carma_guidance_plugins/plugin_base_node.hpp" +#include "TestPlugins.h" + +bool has_publisher(std::shared_ptr node, const std::string& topic_name, const std::string& type) +{ + bool found = false; + for (const auto& endpoint : node->get_publishers_info_by_topic(topic_name)) { + + std::cerr << "name: " << endpoint.node_name() << " type: " << endpoint.topic_type() << std::endl; + + if (endpoint.node_name() == node->get_name() && endpoint.topic_type() == type) + found = true; + } + + return found; +} + +bool has_subscriber(std::shared_ptr node, const std::string& topic_name, const std::string& type) +{ + bool found = false; + for (const auto& endpoint : node->get_subscriptions_info_by_topic(topic_name)) { + + std::cerr << "name: " << endpoint.node_name() << " type: " << endpoint.topic_type() << std::endl; + + if (endpoint.node_name() == node->get_name() && endpoint.topic_type() == type) + found = true; + } + + return found; +} + +bool has_service(std::shared_ptr node, const std::string& service_name, const std::string& type) +{ + bool found = false; + for (const auto& map_item : node->get_service_names_and_types_by_node(node->get_name(), "")) { + + std::cerr << "service: " << map_item.first << " type: " << map_item.second[0] << std::endl; + + if ( map_item.first == service_name && map_item.second[0] == type) + found = true; + } + + return found; +} + +namespace carma_guidance_plugins +{ + +TEST(carma_guidance_plugins_test, connections_test) { + + + std::vector str_remaps = {"--ros-args", "-r", "__node:=strategic_plugin_test"}; + std::vector tac_remaps = {"--ros-args", "-r", "__node:=tactical_plugin_test"}; + std::vector ctrl_remaps = {"--ros-args", "-r", "__node:=control_plugin_test"}; + + rclcpp::NodeOptions str_options; + str_options.arguments(str_remaps); + str_options.use_intra_process_comms(true); + + rclcpp::NodeOptions tac_options; + tac_options.arguments(tac_remaps); + tac_options.use_intra_process_comms(true); + + rclcpp::NodeOptions ctrl_options; + ctrl_options.arguments(ctrl_remaps); + ctrl_options.use_intra_process_comms(true); + + auto strategic_plugin = std::make_shared(str_options); + auto tactical_plugin = std::make_shared(tac_options); + auto control_plugin = std::make_shared(ctrl_options); + + ASSERT_TRUE(strategic_plugin->get_availability()); + ASSERT_TRUE(tactical_plugin->get_availability()); + ASSERT_TRUE(control_plugin->get_availability()); + + ASSERT_EQ(strategic_plugin->get_name(), "TestStrategicPlugin"); + ASSERT_EQ(tactical_plugin->get_name(), "TestTacticalPlugin"); + ASSERT_EQ(control_plugin->get_name(), "TestControlPlugin"); + + ASSERT_EQ(strategic_plugin->get_version_id(), "1.0"); + ASSERT_EQ(tactical_plugin->get_version_id(), "1.1"); + ASSERT_EQ(control_plugin->get_version_id(), "1.2"); + + ASSERT_EQ(strategic_plugin->get_capability(), "strategic_plan/plan_maneuvers/test_capability"); + ASSERT_EQ(tactical_plugin->get_capability(), "tactical_plan/plan_trajectory/test_capability"); + ASSERT_EQ(control_plugin->get_capability(), "control/trajectory_control/test_capability"); + + // Trigger the discovery callback once to ensure discovery publisher is setup + strategic_plugin->discovery_timer_callback(); + tactical_plugin->discovery_timer_callback(); + control_plugin->discovery_timer_callback(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Give a bit for publisher registration to go through + + ASSERT_TRUE(has_publisher(strategic_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_FALSE(has_service(strategic_plugin, "/TestStrategicPlugin/plan_maneuvers", "carma_planning_msgs/srv/PlanManeuvers")); + + ASSERT_TRUE(has_publisher(tactical_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_FALSE(has_service(tactical_plugin, "/TestTacticalPlugin/plan_trajectory", "carma_planning_msgs/srv/PlanTrajectory")); + + ASSERT_TRUE(has_publisher(control_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_FALSE(has_publisher(control_plugin, "ctrl_raw", "autoware_msgs/msg/ControlCommandStamped")); + ASSERT_FALSE(has_subscriber(control_plugin, "current_pose", "geometry_msgs/msg/PoseStamped")); + ASSERT_FALSE(has_subscriber(control_plugin, "vehicle/twist", "geometry_msgs/msg/TwistStamped")); + ASSERT_FALSE(has_subscriber(control_plugin, "TestControlPlugin/plan_trajectory", "carma_planning_msgs/msg/TrajectoryPlan")); + + strategic_plugin->configure(); + tactical_plugin->configure(); + control_plugin->configure(); + + ASSERT_TRUE(has_publisher(strategic_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_TRUE(has_service(strategic_plugin, "/TestStrategicPlugin/plan_maneuvers", "carma_planning_msgs/srv/PlanManeuvers")); + + ASSERT_TRUE(has_publisher(tactical_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); // Only one true before configure is called + ASSERT_TRUE(has_service(tactical_plugin, "/TestTacticalPlugin/plan_trajectory", "carma_planning_msgs/srv/PlanTrajectory")); + + ASSERT_TRUE(has_publisher(control_plugin, "plugin_discovery", "carma_planning_msgs/msg/Plugin")); + ASSERT_TRUE(has_publisher(control_plugin, "ctrl_raw", "autoware_msgs/msg/ControlCommandStamped")); + ASSERT_TRUE(has_subscriber(control_plugin, "current_pose", "geometry_msgs/msg/PoseStamped")); + ASSERT_TRUE(has_subscriber(control_plugin, "vehicle/twist", "geometry_msgs/msg/TwistStamped")); + ASSERT_TRUE(has_subscriber(control_plugin, "TestControlPlugin/plan_trajectory", "carma_planning_msgs/msg/TrajectoryPlan")); + +} + +} // carma_guidance_plugins + +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 From f9c3e1f45e031806c503b6384e6e607ad9b1451b Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Tue, 24 May 2022 22:03:57 +0000 Subject: [PATCH 006/165] WIP making plugin template package --- template_package/carma_package | 100 +++++++++++++++ .../strategic_plugin_node.hpp | 116 ++++++++++++++++++ .../template_package/tactical_plugin_node.hpp | 116 ++++++++++++++++++ .../template_package_node.hpp | 15 +-- 4 files changed, 335 insertions(+), 12 deletions(-) create mode 100644 template_package/include/template_package/strategic_plugin_node.hpp create mode 100644 template_package/include/template_package/tactical_plugin_node.hpp diff --git a/template_package/carma_package b/template_package/carma_package index 92de9914b0..a5c5c883e4 100755 --- a/template_package/carma_package +++ b/template_package/carma_package @@ -50,6 +50,21 @@ if [[ ! -d ${package_parent_path} ]]; then exit 2 fi +plugin_type=$4 + +if [ "$plugin_type" = "strategic" ] || [ "$plugin_type" = "tactical" ] || [ "$plugin_type" = "control" ] + + echo "Making plugin of type: $plugin_type" + +elif [[ -z ${plugin_type} ]]; then + + echo "Plugin type not provided assuming standard node" + plugin_type="none" + +else + echo "Plugin type not valid. Must be one of strategic, tactical, or control. If not making a plugin leave field empty" +fi + target_path=$(realpath ${package_parent_path}/${package_name}) # Copy the template_package to the new package @@ -64,9 +79,94 @@ if [[ $PWD != $(realpath ${target_path}) ]]; then exit 2 fi +plugin_base_overrides = "bool get_availability() override; + + std::string get_name() override; + + std::string get_version_id() override; + + std::string get_capability() override; + + /** + * \brief This method should be used to load parameters and will be called no the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + + /** + * TODO for USER: The following lifecycle overrides are also available if needed + * on_activate_plugin + * on_deactivate_plugin + * on_cleanup_plugin + * on_shutdown_plugin + * on_error_plugin + */" + +if [ "$plugin_type" = "strategic" ] + +base_node_overrides = " void plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override; + + $plugin_base_overrides + " + +base_node = "carma_guidance_plugins::StrategicPlugin" +base_node_header = "#include " + +elif [ "$plugin_type" = "tactical" ] + +base_node_overrides = " void plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr) override; + + $plugin_base_overrides + " + +base_node = "carma_guidance_plugins::TacticalPlugin" +base_node_header = "#include " + +elif [ "$plugin_type" = "control" ] + +base_node_overrides = " autoware_msgs::msg::ControlCommandStamped generate_command() override; + + $plugin_base_overrides + " + +base_node = "carma_guidance_plugins::ControlPlugin" +base_node_header = "#include " + +else + +### Non-Plugin Base Node Substitutions ### +base_node_overrides = " carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + + /** + * TODO for USER: The following lifecycle overrides are also available if needed + * handle_on_activate + * handle_on_deactivate + * handle_on_cleanup + * handle_on_shutdown + * handle_on_error + */ + " + +base_node = "carma_ros2_utils::CarmaLifecycleNode" +base_node_header = "#include " + +fi + + + + + # Perform substitutions find . -type f -exec sed -i "s//${package_name}/g" {} \; find . -type f -exec sed -i "s//${current_year}/g" {} \; +find . -type f -exec sed -i "s//${base_node_overrides}/g" {} \; +find . -type f -exec sed -i "s//${base_node_header}/g" {} \; +find . -type f -exec sed -i "s//${base_node}/g" {} \; # Update file names for new package shopt -s globstar diff --git a/template_package/include/template_package/strategic_plugin_node.hpp b/template_package/include/template_package/strategic_plugin_node.hpp new file mode 100644 index 0000000000..da7068cae1 --- /dev/null +++ b/template_package/include/template_package/strategic_plugin_node.hpp @@ -0,0 +1,116 @@ +/* + * Copyright (C) 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. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include "/_config.hpp" + +namespace +{ + + /** + * \brief TODO for USER: Add class description + * + */ + class Node : public carma_guidance_plugins::StrategicPlugin + { + + private: + // Subscribers + carma_ros2_utils::SubPtr example_sub_; + + // Publishers + carma_ros2_utils::PubPtr example_pub_; + + // Service Clients + carma_ros2_utils::ClientPtr example_client_; + + // Service Servers + carma_ros2_utils::ServicePtr example_service_; + + // Timers + rclcpp::TimerBase::SharedPtr example_timer_; + + // Node configuration + Config config_; + + public: + /** + * \brief Node constructor + */ + explicit Node(const rclcpp::NodeOptions &); + + /** + * \brief Example callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief Example timer callback + */ + void example_timer_callback(); + + /** + * \brief Example subscription callback + */ + void example_callback(std_msgs::msg::String::UniquePtr msg); + + /** + * \brief Example service callback + */ + void example_service_callback(const std::shared_ptr header, + const std::shared_ptr request, + std::shared_ptr response); + + //// + // Overrides + //// + void plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override; + + bool get_availability() override; + + std::string get_name() override; + + std::string get_version_id() override; + + std::string get_capability() override; + + /** + * \brief This method should be used to load parameters and will be called no the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + + /** + * TODO for USER: The following lifecycle overrides are also available if needed + * on_activate_plugin + * on_deactivate_plugin + * on_cleanup_plugin + * on_shutdown_plugin + * on_error_plugin + */ + }; + +} // diff --git a/template_package/include/template_package/tactical_plugin_node.hpp b/template_package/include/template_package/tactical_plugin_node.hpp new file mode 100644 index 0000000000..3980a6dec5 --- /dev/null +++ b/template_package/include/template_package/tactical_plugin_node.hpp @@ -0,0 +1,116 @@ +/* + * Copyright (C) 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. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include "/_config.hpp" + +namespace +{ + + /** + * \brief TODO for USER: Add class description + * + */ + class Node : public carma_guidance_plugins::TacticalPlugin + { + + private: + // Subscribers + carma_ros2_utils::SubPtr example_sub_; + + // Publishers + carma_ros2_utils::PubPtr example_pub_; + + // Service Clients + carma_ros2_utils::ClientPtr example_client_; + + // Service Servers + carma_ros2_utils::ServicePtr example_service_; + + // Timers + rclcpp::TimerBase::SharedPtr example_timer_; + + // Node configuration + Config config_; + + public: + /** + * \brief Node constructor + */ + explicit Node(const rclcpp::NodeOptions &); + + /** + * \brief Example callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief Example timer callback + */ + void example_timer_callback(); + + /** + * \brief Example subscription callback + */ + void example_callback(std_msgs::msg::String::UniquePtr msg); + + /** + * \brief Example service callback + */ + void example_service_callback(const std::shared_ptr header, + const std::shared_ptr request, + std::shared_ptr response); + + //// + // Overrides + //// + void plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override; + + bool get_availability() override; + + std::string get_name() override; + + std::string get_version_id() override; + + std::string get_capability() override; + + /** + * \brief This method should be used to load parameters and will be called no the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + + /** + * TODO for USER: The following lifecycle overrides are also available if needed + * on_activate_plugin + * on_deactivate_plugin + * on_cleanup_plugin + * on_shutdown_plugin + * on_error_plugin + */ + }; + +} // diff --git a/template_package/include/template_package/template_package_node.hpp b/template_package/include/template_package/template_package_node.hpp index d7cbf2708b..65a86c40b0 100644 --- a/template_package/include/template_package/template_package_node.hpp +++ b/template_package/include/template_package/template_package_node.hpp @@ -21,7 +21,7 @@ #include #include -#include + #include "/_config.hpp" namespace @@ -31,7 +31,7 @@ namespace * \brief TODO for USER: Add class description * */ - class Node : public carma_ros2_utils::CarmaLifecycleNode + class Node : public { private: @@ -85,16 +85,7 @@ namespace //// // Overrides //// - carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); - - /** - * TODO for USER: The following lifecycle overrides are also available if needed - * handle_on_activate - * handle_on_deactivate - * handle_on_cleanup - * handle_on_shutdown - * handle_on_error - */ + }; } // From 7c885582d9348bb246dc58ac1865062564e721f4 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Wed, 25 May 2022 18:29:34 +0000 Subject: [PATCH 007/165] wip sortaworks for headers --- template_package/carma_package | 125 ++++++++++++++++----------------- 1 file changed, 61 insertions(+), 64 deletions(-) diff --git a/template_package/carma_package b/template_package/carma_package index a5c5c883e4..46c9b91d02 100755 --- a/template_package/carma_package +++ b/template_package/carma_package @@ -52,7 +52,7 @@ fi plugin_type=$4 -if [ "$plugin_type" = "strategic" ] || [ "$plugin_type" = "tactical" ] || [ "$plugin_type" = "control" ] +if [ "$plugin_type" = "strategic" ] || [ "$plugin_type" = "tactical" ] || [ "$plugin_type" = "control" ]; then echo "Making plugin of type: $plugin_type" @@ -79,94 +79,91 @@ if [[ $PWD != $(realpath ${target_path}) ]]; then exit 2 fi -plugin_base_overrides = "bool get_availability() override; - - std::string get_name() override; - - std::string get_version_id() override; +plugin_base_overrides="bool get_availability() override;\ +\ + std::string get_name() override;\ +\ + std::string get_version_id() override;\ +\ + std::string get_capability() override;\ + \ + /**\ + * \brief This method should be used to load parameters and will be called no the configure state transition.\ + */ \ + carma_ros2_utils::CallbackReturn on_configure_plugin();\ +\ + /**\ + * TODO for USER: The following lifecycle overrides are also available if needed\ + * on_activate_plugin\ + * on_deactivate_plugin\ + * on_cleanup_plugin\ + * on_shutdown_plugin\ + * on_error_plugin\ + */\ + " - std::string get_capability() override; - - /** - * \brief This method should be used to load parameters and will be called no the configure state transition. - */ - carma_ros2_utils::CallbackReturn on_configure_plugin(); - - /** - * TODO for USER: The following lifecycle overrides are also available if needed - * on_activate_plugin - * on_deactivate_plugin - * on_cleanup_plugin - * on_shutdown_plugin - * on_error_plugin - */" - -if [ "$plugin_type" = "strategic" ] - -base_node_overrides = " void plan_maneuvers_callback( - std::shared_ptr, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, - carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override; +if [ "$plugin_type" = "strategic" ]; then +base_node_overrides="void plan_maneuvers_callback(\ + std::shared_ptr, \ + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, \ + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override;\ +\ $plugin_base_overrides " -base_node = "carma_guidance_plugins::StrategicPlugin" -base_node_header = "#include " - -elif [ "$plugin_type" = "tactical" ] +base_node='carma_guidance_plugins::StrategicPlugin' +base_node_header='#include ' -base_node_overrides = " void plan_trajectory_callback( - std::shared_ptr, - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr, - carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr) override; +elif [ "$plugin_type" = "tactical" ]; then +base_node_overrides="void plan_trajectory_callback(\ + std::shared_ptr, \ + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr, \ + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr) override;\ +\ $plugin_base_overrides " -base_node = "carma_guidance_plugins::TacticalPlugin" -base_node_header = "#include " - -elif [ "$plugin_type" = "control" ] +base_node='carma_guidance_plugins::TacticalPlugin' +base_node_header='#include ' -base_node_overrides = " autoware_msgs::msg::ControlCommandStamped generate_command() override; +elif [ "$plugin_type" = "control" ]; then +base_node_overrides="autoware_msgs::msg::ControlCommandStamped generate_command() override;\ +\ $plugin_base_overrides " -base_node = "carma_guidance_plugins::ControlPlugin" -base_node_header = "#include " +base_node='carma_guidance_plugins::ControlPlugin' +base_node_header='#include ' else ### Non-Plugin Base Node Substitutions ### -base_node_overrides = " carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); - - /** - * TODO for USER: The following lifecycle overrides are also available if needed - * handle_on_activate - * handle_on_deactivate - * handle_on_cleanup - * handle_on_shutdown - * handle_on_error - */ - " - -base_node = "carma_ros2_utils::CarmaLifecycleNode" -base_node_header = "#include " +base_node_overrides='carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State \&prev_state);\ +\ + /**\ + * TODO for USER: The following lifecycle overrides are also available if needed\ + * handle_on_activate\ + * handle_on_deactivate\ + * handle_on_cleanup\ + * handle_on_shutdown\ + * handle_on_error\ + */\ + ' + +base_node='carma_ros2_utils::CarmaLifecycleNode' +base_node_header='#include ' fi - - - - # Perform substitutions find . -type f -exec sed -i "s//${package_name}/g" {} \; find . -type f -exec sed -i "s//${current_year}/g" {} \; -find . -type f -exec sed -i "s//${base_node_overrides}/g" {} \; -find . -type f -exec sed -i "s//${base_node_header}/g" {} \; -find . -type f -exec sed -i "s//${base_node}/g" {} \; +find . -type f -exec sed -i "s||${base_node_overrides}|g" {} \; +find . -type f -exec sed -i "s||${base_node_header}|g" {} \; +find . -type f -exec sed -i "s||${base_node}|g" {} \; # Update file names for new package shopt -s globstar From 6e542ead7a41085e11528495995cef81808dd2f1 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Wed, 25 May 2022 22:01:40 +0000 Subject: [PATCH 008/165] revert --- .../template_package/template_package_node.hpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/template_package/include/template_package/template_package_node.hpp b/template_package/include/template_package/template_package_node.hpp index 65a86c40b0..d7cbf2708b 100644 --- a/template_package/include/template_package/template_package_node.hpp +++ b/template_package/include/template_package/template_package_node.hpp @@ -21,7 +21,7 @@ #include #include - +#include #include "/_config.hpp" namespace @@ -31,7 +31,7 @@ namespace * \brief TODO for USER: Add class description * */ - class Node : public + class Node : public carma_ros2_utils::CarmaLifecycleNode { private: @@ -85,7 +85,16 @@ namespace //// // Overrides //// - + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + + /** + * TODO for USER: The following lifecycle overrides are also available if needed + * handle_on_activate + * handle_on_deactivate + * handle_on_cleanup + * handle_on_shutdown + * handle_on_error + */ }; } // From 2b2e7de755b982fcd8959487b6c3f4d7be2dfce7 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Thu, 26 May 2022 02:07:17 +0000 Subject: [PATCH 009/165] Working template package for ROS2 plugins --- template_package/README.md | 20 ++- template_package/carma_package | 113 +++++++--------- .../template_control_plugin_node.hpp | 112 +++++++++++++++ .../template_package_node.hpp | 2 +- ...hpp => template_strategic_plugin_node.hpp} | 9 +- ....hpp => template_tactical_plugin_node.hpp} | 13 +- template_package/package.xml | 1 + .../src/template_control_plugin_node.cpp | 126 +++++++++++++++++ .../src/template_package_node.cpp | 2 +- .../src/template_strategic_plugin_node.cpp | 127 ++++++++++++++++++ .../src/template_tactical_plugin_node.cpp | 127 ++++++++++++++++++ 11 files changed, 565 insertions(+), 87 deletions(-) create mode 100644 template_package/include/template_package/template_control_plugin_node.hpp rename template_package/include/template_package/{strategic_plugin_node.hpp => template_strategic_plugin_node.hpp} (96%) rename template_package/include/template_package/{tactical_plugin_node.hpp => template_tactical_plugin_node.hpp} (89%) create mode 100644 template_package/src/template_control_plugin_node.cpp create mode 100644 template_package/src/template_strategic_plugin_node.cpp create mode 100644 template_package/src/template_tactical_plugin_node.cpp diff --git a/template_package/README.md b/template_package/README.md index 0e32fdae93..aa7e8f329c 100644 --- a/template_package/README.md +++ b/template_package/README.md @@ -1,19 +1,22 @@ # template_package -This package serves as a template which can be copy and pasted to create a new ROS2 Node following CARMA Platform's default structure. +This package serves as a template which can used to create a new ROS2 Node or Guidance Plugin Node following CARMA Platform's default structure. The contents of this package are not build-able on their own. There are a number of substitution parameters that must be replaced with the desired package name these are indicated with a `````` indicator in the text. The following substitutions are used internally: + - `````` The new package name - `````` The current year +- `````` The dep to use as a base node -To make the process easier a script is provided to create the new package with the desired package name and current year. +To make the process easier a script is provided to create the new package of the desired type with the desired package name and current year. -# Creating a new ROS2 package for CARMA Platform +## Creating a new ROS2 package for CARMA Platform ```bash -./carma_package +./carma_package +# If set, should be one of strategic, tactical, or control. If unset, then not a plugin" ``` -For example, if you have a standard carma source checkout: +For example, if you have a standard carma source checkout and want to make a regular node: ```bash cd src/carma-platform/template_package @@ -26,4 +29,11 @@ If successful you should see the new location printed. my_package created at /workspaces/carma_ws/src/carma-platform/my_package ``` +To make a new Guidance Plugin, you must specify the plugin type: + +```bash +cd src/carma-platform/template_package +./carma_package . my_package ../ strategic +``` + This new package can now be built like any other carma package. diff --git a/template_package/carma_package b/template_package/carma_package index 46c9b91d02..f49cd62d9e 100755 --- a/template_package/carma_package +++ b/template_package/carma_package @@ -5,7 +5,8 @@ set -e # This script is used to build a package for the CARMA platform. # The script will create a new folder in the parent folder with the name of the package. # After calling the script the new package should be immeadiatly buildable -# Usage: ./carma_package +# Usage: ./carma_package +# should be one of strategic, tactical, or control. If unset, then not a plugin if [[ -z $BASH_VERSION ]]; then echo "Bash version could not be checked. Please run this script using bash" @@ -19,7 +20,7 @@ fi if [[ "$#" -ne 3 ]]; then - echo "Incorrect inputs: Input must be of form " + echo "Incorrect inputs: Input must be of form . If set, should be one of strategic, tactical, or control. If unset, then not a plugin" exit 2 fi @@ -55,14 +56,18 @@ plugin_type=$4 if [ "$plugin_type" = "strategic" ] || [ "$plugin_type" = "tactical" ] || [ "$plugin_type" = "control" ]; then echo "Making plugin of type: $plugin_type" + + base_node_dep='carma_guidance_plugins<\/depend>' elif [[ -z ${plugin_type} ]]; then - echo "Plugin type not provided assuming standard node" + echo "Plugin type not provided assuming standard node (not a plugin)" plugin_type="none" + base_node_dep='' else echo "Plugin type not valid. Must be one of strategic, tactical, or control. If not making a plugin leave field empty" + exit 2 fi target_path=$(realpath ${package_parent_path}/${package_name}) @@ -79,91 +84,56 @@ if [[ $PWD != $(realpath ${target_path}) ]]; then exit 2 fi -plugin_base_overrides="bool get_availability() override;\ -\ - std::string get_name() override;\ -\ - std::string get_version_id() override;\ -\ - std::string get_capability() override;\ - \ - /**\ - * \brief This method should be used to load parameters and will be called no the configure state transition.\ - */ \ - carma_ros2_utils::CallbackReturn on_configure_plugin();\ -\ - /**\ - * TODO for USER: The following lifecycle overrides are also available if needed\ - * on_activate_plugin\ - * on_deactivate_plugin\ - * on_cleanup_plugin\ - * on_shutdown_plugin\ - * on_error_plugin\ - */\ - " - +# Cleanup alternative node files based on user specified type befoe substitutions if [ "$plugin_type" = "strategic" ]; then -base_node_overrides="void plan_maneuvers_callback(\ - std::shared_ptr, \ - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, \ - carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override;\ -\ - $plugin_base_overrides - " + rm include/template_package/template_package_node.hpp + rm include/template_package/template_tactical_plugin_node.hpp + rm include/template_package/template_control_plugin_node.hpp -base_node='carma_guidance_plugins::StrategicPlugin' -base_node_header='#include ' + rm src/template_package_node.cpp + rm src/template_tactical_plugin_node.cpp + rm src/template_control_plugin_node.cpp elif [ "$plugin_type" = "tactical" ]; then -base_node_overrides="void plan_trajectory_callback(\ - std::shared_ptr, \ - carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr, \ - carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr) override;\ -\ - $plugin_base_overrides - " + rm include/template_package/template_package_node.hpp + rm include/template_package/template_strategic_plugin_node.hpp + rm include/template_package/template_control_plugin_node.hpp -base_node='carma_guidance_plugins::TacticalPlugin' -base_node_header='#include ' + rm src/template_package_node.cpp + rm src/template_strategic_plugin_node.cpp + rm src/template_control_plugin_node.cpp elif [ "$plugin_type" = "control" ]; then -base_node_overrides="autoware_msgs::msg::ControlCommandStamped generate_command() override;\ -\ - $plugin_base_overrides - " + rm include/template_package/template_package_node.hpp + rm include/template_package/template_strategic_plugin_node.hpp + rm include/template_package/template_tactical_plugin_node.hpp -base_node='carma_guidance_plugins::ControlPlugin' -base_node_header='#include ' + rm src/template_package_node.cpp + rm src/template_strategic_plugin_node.cpp + rm src/template_tactical_plugin_node.cpp else -### Non-Plugin Base Node Substitutions ### -base_node_overrides='carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State \&prev_state);\ -\ - /**\ - * TODO for USER: The following lifecycle overrides are also available if needed\ - * handle_on_activate\ - * handle_on_deactivate\ - * handle_on_cleanup\ - * handle_on_shutdown\ - * handle_on_error\ - */\ - ' - -base_node='carma_ros2_utils::CarmaLifecycleNode' -base_node_header='#include ' + rm include/template_package/template_strategic_plugin_node.hpp + rm include/template_package/template_tactical_plugin_node.hpp + rm include/template_package/template_control_plugin_node.hpp + + rm src/template_strategic_plugin_node.cpp + rm src/template_tactical_plugin_node.cpp + rm src/template_control_plugin_node.cpp fi # Perform substitutions find . -type f -exec sed -i "s//${package_name}/g" {} \; find . -type f -exec sed -i "s//${current_year}/g" {} \; -find . -type f -exec sed -i "s||${base_node_overrides}|g" {} \; -find . -type f -exec sed -i "s||${base_node_header}|g" {} \; -find . -type f -exec sed -i "s||${base_node}|g" {} \; +find . -type f -exec sed -i "s//${base_node_dep}/g" {} \; +#find . -type f -exec sed -i "s//${world_model_dep}/g" {} \; +#find . -type f -exec sed -i "s//${autonomy_lib_dep}/g" {} \; + # Update file names for new package shopt -s globstar @@ -173,7 +143,14 @@ for f in ./**; do continue fi + # Here the files will be renamed. + # Since the user must specify one of none, strategic, tactical, or control and we delete the conflicting files above + # We should be able to safely replace all the rules even when unused without conflict new_name=$(echo $f | sed "s/template_package/${package_name}/g") + new_name=$(echo $new_name | sed "s/template_strategic_plugin/${package_name}/g") + new_name=$(echo $new_name | sed "s/template_tactical_plugin/${package_name}/g") + new_name=$(echo $new_name | sed "s/template_control_plugin/${package_name}/g") + directory_name=$(dirname $new_name) if [[ ! -d $directory_name ]]; then mkdir -p $directory_name diff --git a/template_package/include/template_package/template_control_plugin_node.hpp b/template_package/include/template_package/template_control_plugin_node.hpp new file mode 100644 index 0000000000..6fdf1c5be5 --- /dev/null +++ b/template_package/include/template_package/template_control_plugin_node.hpp @@ -0,0 +1,112 @@ +/* + * Copyright (C) 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. + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include "/_config.hpp" + +namespace +{ + + /** + * \brief TODO for USER: Add class description + * + */ + class Node : public carma_guidance_plugins::ControlPlugin + { + + private: + // Subscribers + carma_ros2_utils::SubPtr example_sub_; + + // Publishers + carma_ros2_utils::PubPtr example_pub_; + + // Service Clients + carma_ros2_utils::ClientPtr example_client_; + + // Service Servers + carma_ros2_utils::ServicePtr example_service_; + + // Timers + rclcpp::TimerBase::SharedPtr example_timer_; + + // Node configuration + Config config_; + + public: + /** + * \brief Node constructor + */ + explicit Node(const rclcpp::NodeOptions &); + + /** + * \brief Example callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief Example timer callback + */ + void example_timer_callback(); + + /** + * \brief Example subscription callback + */ + void example_callback(std_msgs::msg::String::UniquePtr msg); + + /** + * \brief Example service callback + */ + void example_service_callback(const std::shared_ptr header, + const std::shared_ptr request, + std::shared_ptr response); + + //// + // Overrides + //// + autoware_msgs::msg::ControlCommandStamped generate_command() override; + + bool get_availability() override; + + std::string get_name() override; + + std::string get_version_id() override; + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + + /** + * TODO for USER: The following lifecycle overrides are also available if needed + * get_capability + * on_activate_plugin + * on_deactivate_plugin + * on_cleanup_plugin + * on_shutdown_plugin + * on_error_plugin + */ + }; + +} // diff --git a/template_package/include/template_package/template_package_node.hpp b/template_package/include/template_package/template_package_node.hpp index d7cbf2708b..5639014be2 100644 --- a/template_package/include/template_package/template_package_node.hpp +++ b/template_package/include/template_package/template_package_node.hpp @@ -85,7 +85,7 @@ namespace //// // Overrides //// - carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); /** * TODO for USER: The following lifecycle overrides are also available if needed diff --git a/template_package/include/template_package/strategic_plugin_node.hpp b/template_package/include/template_package/template_strategic_plugin_node.hpp similarity index 96% rename from template_package/include/template_package/strategic_plugin_node.hpp rename to template_package/include/template_package/template_strategic_plugin_node.hpp index da7068cae1..1360ee210d 100644 --- a/template_package/include/template_package/strategic_plugin_node.hpp +++ b/template_package/include/template_package/template_strategic_plugin_node.hpp @@ -87,24 +87,23 @@ namespace //// void plan_maneuvers_callback( std::shared_ptr, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, - carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override; + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) override; bool get_availability() override; std::string get_name() override; std::string get_version_id() override; - - std::string get_capability() override; /** - * \brief This method should be used to load parameters and will be called no the configure state transition. + * \brief This method should be used to load parameters and will be called on the configure state transition. */ carma_ros2_utils::CallbackReturn on_configure_plugin(); /** * TODO for USER: The following lifecycle overrides are also available if needed + * get_capability * on_activate_plugin * on_deactivate_plugin * on_cleanup_plugin diff --git a/template_package/include/template_package/tactical_plugin_node.hpp b/template_package/include/template_package/template_tactical_plugin_node.hpp similarity index 89% rename from template_package/include/template_package/tactical_plugin_node.hpp rename to template_package/include/template_package/template_tactical_plugin_node.hpp index 3980a6dec5..e5d9ebf347 100644 --- a/template_package/include/template_package/tactical_plugin_node.hpp +++ b/template_package/include/template_package/template_tactical_plugin_node.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include "/_config.hpp" namespace @@ -85,26 +85,25 @@ namespace //// // Overrides //// - void plan_maneuvers_callback( + void plan_trajectory_callback( std::shared_ptr, - carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr, - carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr) override; + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; bool get_availability() override; std::string get_name() override; std::string get_version_id() override; - - std::string get_capability() override; /** - * \brief This method should be used to load parameters and will be called no the configure state transition. + * \brief This method should be used to load parameters and will be called on the configure state transition. */ carma_ros2_utils::CallbackReturn on_configure_plugin(); /** * TODO for USER: The following lifecycle overrides are also available if needed + * get_capability * on_activate_plugin * on_deactivate_plugin * on_cleanup_plugin diff --git a/template_package/package.xml b/template_package/package.xml index d493e5a398..a487dcad99 100644 --- a/template_package/package.xml +++ b/template_package/package.xml @@ -30,6 +30,7 @@ carma_ros2_utils rclcpp_components std_srvs + ament_lint_auto ament_cmake_gtest diff --git a/template_package/src/template_control_plugin_node.cpp b/template_package/src/template_control_plugin_node.cpp new file mode 100644 index 0000000000..03521cce57 --- /dev/null +++ b/template_package/src/template_control_plugin_node.cpp @@ -0,0 +1,126 @@ +/* + * Copyright (C) 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 "/_node.hpp" + +namespace +{ + namespace std_ph = std::placeholders; + + Node::Node(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::ControlPlugin(options) + { + // Create initial config + config_ = Config(); + + // Declare parameters + config_.example_param = declare_parameter("example_param", config_.example_param); + } + + rcl_interfaces::msg::SetParametersResult Node::parameter_update_callback(const std::vector ¶meters) + { + // TODO for the USER: Ensure all parameters can be updated dynamically by adding them to this method + auto error = update_params({{"example_param", config_.example_param}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error; + + return result; + } + + carma_ros2_utils::CallbackReturn Node::on_configure_plugin() + { + // Reset config + config_ = Config(); + + // Load parameters + get_parameter("example_param", config_.example_param); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1)); + + // Setup subscribers + example_sub_ = create_subscription("example_input_topic", 10, + std::bind(&Node::example_callback, this, std_ph::_1)); + + // Setup publishers + example_pub_ = create_publisher("example_output_topic", 10); + + // Setup service clients + example_client_ = create_client("example_used_service"); + + // Setup service servers + example_service_ = create_service("example_provided_service", + std::bind(&Node::example_service_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); + + // Setup timers + // NOTE: You will not be able to actually publish until in the ACTIVE state + // so it may often make more sense for timers to be created in handle_on_activate + example_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(1000), + std::bind(&Node::example_timer_callback, this)); + + // Return success if everthing initialized successfully + return CallbackReturn::SUCCESS; + } + + // Parameter names not shown to prevent unused compile warning. The user may add them back + void Node::example_service_callback(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) + { + RCLCPP_INFO( get_logger(), "Example service callback"); + } + + void Node::example_timer_callback() + { + RCLCPP_DEBUG(get_logger(), "Example timer callback"); + std_msgs::msg::String msg; + msg.data = "Hello World!"; + example_pub_->publish(msg); + } + + void Node::example_callback(std_msgs::msg::String::UniquePtr msg) + { + RCLCPP_INFO_STREAM( get_logger(), "example_sub_ callback called with value: " << msg->data); + } + + autoware_msgs::msg::ControlCommandStamped Node::generate_command() + { + autoware_msgs::msg::ControlCommandStamped msg; + // TODO for user: Implement logic to generate control commands here + return msg; + } + + bool Node::get_availability() { + return true; // TODO for user implement actual check on availability if applicable to plugin + } + + std::string Node::get_name() { + return "TODO for user specify plugin name here"; + } + + std::string Node::get_version_id() { + return "TODO for user specify plugin version id here"; + } + +} // + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(::Node) diff --git a/template_package/src/template_package_node.cpp b/template_package/src/template_package_node.cpp index 5d50bf45af..fcdc2984a8 100644 --- a/template_package/src/template_package_node.cpp +++ b/template_package/src/template_package_node.cpp @@ -74,7 +74,7 @@ namespace std::chrono::milliseconds(1000), std::bind(&Node::example_timer_callback, this)); - // Return success if everthing initialized successfully + // Return success if everything initialized successfully return CallbackReturn::SUCCESS; } diff --git a/template_package/src/template_strategic_plugin_node.cpp b/template_package/src/template_strategic_plugin_node.cpp new file mode 100644 index 0000000000..d698c22fd4 --- /dev/null +++ b/template_package/src/template_strategic_plugin_node.cpp @@ -0,0 +1,127 @@ +/* + * Copyright (C) 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 "/_node.hpp" + +namespace +{ + namespace std_ph = std::placeholders; + + Node::Node(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::StrategicPlugin(options) + { + // Create initial config + config_ = Config(); + + // Declare parameters + config_.example_param = declare_parameter("example_param", config_.example_param); + } + + rcl_interfaces::msg::SetParametersResult Node::parameter_update_callback(const std::vector ¶meters) + { + // TODO for the USER: Ensure all parameters can be updated dynamically by adding them to this method + auto error = update_params({{"example_param", config_.example_param}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error; + + return result; + } + + carma_ros2_utils::CallbackReturn Node::on_configure_plugin() + { + // Reset config + config_ = Config(); + + // Load parameters + get_parameter("example_param", config_.example_param); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1)); + + // Setup subscribers + example_sub_ = create_subscription("example_input_topic", 10, + std::bind(&Node::example_callback, this, std_ph::_1)); + + // Setup publishers + example_pub_ = create_publisher("example_output_topic", 10); + + // Setup service clients + example_client_ = create_client("example_used_service"); + + // Setup service servers + example_service_ = create_service("example_provided_service", + std::bind(&Node::example_service_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); + + // Setup timers + // NOTE: You will not be able to actually publish until in the ACTIVE state + // so it may often make more sense for timers to be created in handle_on_activate + example_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(1000), + std::bind(&Node::example_timer_callback, this)); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + // Parameter names not shown to prevent unused compile warning. The user may add them back + void Node::example_service_callback(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) + { + RCLCPP_INFO( get_logger(), "Example service callback"); + } + + void Node::example_timer_callback() + { + RCLCPP_DEBUG(get_logger(), "Example timer callback"); + std_msgs::msg::String msg; + msg.data = "Hello World!"; + example_pub_->publish(msg); + } + + void Node::example_callback(std_msgs::msg::String::UniquePtr msg) + { + RCLCPP_INFO_STREAM( get_logger(), "example_sub_ callback called with value: " << msg->data); + } + + void Node::plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) + { + // TODO for user: Implement maneuver planning logic here to populate resp based on req. + } + + bool Node::get_availability() { + return true; // TODO for user implement actual check on availability if applicable to plugin + } + + std::string Node::get_name() { + return "TODO for user specify plugin name here"; + } + + std::string Node::get_version_id() { + return "TODO for user specify plugin version id here"; + } + +} // + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(::Node) diff --git a/template_package/src/template_tactical_plugin_node.cpp b/template_package/src/template_tactical_plugin_node.cpp new file mode 100644 index 0000000000..697a6fedfa --- /dev/null +++ b/template_package/src/template_tactical_plugin_node.cpp @@ -0,0 +1,127 @@ +/* + * Copyright (C) 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 "/_node.hpp" + +namespace +{ + namespace std_ph = std::placeholders; + + Node::Node(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::TacticalPlugin(options) + { + // Create initial config + config_ = Config(); + + // Declare parameters + config_.example_param = declare_parameter("example_param", config_.example_param); + } + + rcl_interfaces::msg::SetParametersResult Node::parameter_update_callback(const std::vector ¶meters) + { + // TODO for the USER: Ensure all parameters can be updated dynamically by adding them to this method + auto error = update_params({{"example_param", config_.example_param}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error; + + return result; + } + + carma_ros2_utils::CallbackReturn Node::on_configure_plugin() + { + // Reset config + config_ = Config(); + + // Load parameters + get_parameter("example_param", config_.example_param); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1)); + + // Setup subscribers + example_sub_ = create_subscription("example_input_topic", 10, + std::bind(&Node::example_callback, this, std_ph::_1)); + + // Setup publishers + example_pub_ = create_publisher("example_output_topic", 10); + + // Setup service clients + example_client_ = create_client("example_used_service"); + + // Setup service servers + example_service_ = create_service("example_provided_service", + std::bind(&Node::example_service_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); + + // Setup timers + // NOTE: You will not be able to actually publish until in the ACTIVE state + // so it may often make more sense for timers to be created in handle_on_activate + example_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(1000), + std::bind(&Node::example_timer_callback, this)); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + // Parameter names not shown to prevent unused compile warning. The user may add them back + void Node::example_service_callback(const std::shared_ptr, + const std::shared_ptr, + std::shared_ptr) + { + RCLCPP_INFO( get_logger(), "Example service callback"); + } + + void Node::example_timer_callback() + { + RCLCPP_DEBUG(get_logger(), "Example timer callback"); + std_msgs::msg::String msg; + msg.data = "Hello World!"; + example_pub_->publish(msg); + } + + void Node::example_callback(std_msgs::msg::String::UniquePtr msg) + { + RCLCPP_INFO_STREAM( get_logger(), "example_sub_ callback called with value: " << msg->data); + } + + void Node::plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) + { + // TODO for user: Implement trajectory planning logic here by populating resp base on req. + } + + bool Node::get_availability() { + return true; // TODO for user implement actual check on availability if applicable to plugin + } + + std::string Node::get_name() { + return "TODO for user specify plugin name here"; + } + + std::string Node::get_version_id() { + return "TODO for user specify plugin version id here"; + } + +} // + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(::Node) From a40e0f9f66099a85e71e3b2eb08d26d5f24054c5 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Thu, 26 May 2022 02:33:45 +0000 Subject: [PATCH 010/165] add world model and sonar scan file updates --- .sonarqube/sonar-scanner.properties | 8 ++++++-- .../include/carma_guidance_plugins/plugin_base_node.hpp | 8 ++++++++ carma_guidance_plugins/package.xml | 1 + carma_guidance_plugins/src/plugin_base_node.cpp | 2 +- 4 files changed, 16 insertions(+), 3 deletions(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 5658936a30..d361e1b5aa 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -72,7 +72,8 @@ sonar.modules= bsm_generator, \ frame_transformer, \ object_visualizer, \ points_map_filter, \ - light_controlled_intersection_tactical_plugin + light_controlled_intersection_tactical_plugin, \ + carma_guidance_plugins guidance.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/guidance bsm_generator.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/bsm_generator @@ -118,6 +119,7 @@ object_visualizer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/object_vis points_map_filter.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/points_map_filter light_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/light_controlled_intersection_tactical_plugin lci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/lci_strategic_plugin +carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_guidance_plugins # C++ Package differences # Sources @@ -165,6 +167,7 @@ object_visualizer.sonar.sources = src points_map_filter.sonar.sources = src light_controlled_intersection_tactical_plugin.sonar.sources = src lci_strategic_plugin.sonar.sources = src +carma_guidance_plugins.sonar.sources = src # Tests # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated. @@ -208,4 +211,5 @@ frame_transformer.sonar.sources = test object_visualizer.sonar.sources = test points_map_filter.sonar.sources = test light_controlled_intersection_tactical_plugin.sonar.tests = test -lci_strategic_plugin.sonar.sources = src +lci_strategic_plugin.sonar.sources = test +carma_guidance_plugins.sonar.sources = test diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp index 9eb7f7183d..e6622939f3 100644 --- a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp +++ b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include @@ -46,6 +48,12 @@ namespace carma_guidance_plugins //! Timer to trigger publication of the plugin discovery message at a fixed frequency rclcpp::TimerBase::SharedPtr discovert_timer_; + // WorldModel listener + carma_wm::WMListener wm_listener_; + + // World Model populated by the listener at runtime + carma_wm::WorldModelConstPtr wm_; + /** * \brief Callback for the plugin discovery timer which will publish the plugin discovery message */ diff --git a/carma_guidance_plugins/package.xml b/carma_guidance_plugins/package.xml index e69e405497..9db826f225 100644 --- a/carma_guidance_plugins/package.xml +++ b/carma_guidance_plugins/package.xml @@ -30,6 +30,7 @@ carma_ros2_utils carma_planning_msgs autoware_msgs + carma_wm_ros2 ament_lint_auto ament_cmake_gtest diff --git a/carma_guidance_plugins/src/plugin_base_node.cpp b/carma_guidance_plugins/src/plugin_base_node.cpp index 00e87c9019..e271120305 100644 --- a/carma_guidance_plugins/src/plugin_base_node.cpp +++ b/carma_guidance_plugins/src/plugin_base_node.cpp @@ -23,7 +23,7 @@ namespace carma_guidance_plugins namespace std_ph = std::placeholders; PluginBaseNode::PluginBaseNode(const rclcpp::NodeOptions &options) - : carma_ros2_utils::CarmaLifecycleNode(options) + : carma_ros2_utils::CarmaLifecycleNode(options), wm_(wm_listener_.getWorldModel()) { // Setup discovery timer to publish onto the plugin_discovery_pub From b6d4fe3e3b917295b582e041dfed0e8b072d79a7 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Thu, 26 May 2022 02:46:12 +0000 Subject: [PATCH 011/165] fix build error --- carma_guidance_plugins/src/plugin_base_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/carma_guidance_plugins/src/plugin_base_node.cpp b/carma_guidance_plugins/src/plugin_base_node.cpp index e271120305..c67db599b8 100644 --- a/carma_guidance_plugins/src/plugin_base_node.cpp +++ b/carma_guidance_plugins/src/plugin_base_node.cpp @@ -23,7 +23,9 @@ namespace carma_guidance_plugins namespace std_ph = std::placeholders; PluginBaseNode::PluginBaseNode(const rclcpp::NodeOptions &options) - : carma_ros2_utils::CarmaLifecycleNode(options), wm_(wm_listener_.getWorldModel()) + : carma_ros2_utils::CarmaLifecycleNode(options), + wm_listener_(this->get_node_base_interface(), this->get_node_logging_interface(), this->get_node_topics_interface(), this->get_node_parameters_interface()), + wm_(wm_listener_.getWorldModel()) { // Setup discovery timer to publish onto the plugin_discovery_pub From a43fc0c0ec323124a7dde3c715ed9e5fb84c682f Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Tue, 31 May 2022 20:43:48 +0000 Subject: [PATCH 012/165] WIP --- .../base_subsystem_controller.hpp | 11 +++ .../guidance_controller.hpp | 3 + .../guidance_controller_config.hpp | 64 ++++++++++++++++++ .../guidance_controller/plugin_manager.h | 33 ++++----- .../base_subsystem_controller.cpp | 16 +++++ .../guidance_controller.cpp | 67 +++++++++++++++++-- .../guidance_controller/plugin_manager.cpp | 39 ++++++++--- 7 files changed, 200 insertions(+), 33 deletions(-) create mode 100644 subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp diff --git a/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp b/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp index 48272a39fe..00c1e42911 100644 --- a/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp @@ -94,6 +94,17 @@ namespace subsystem_controllers //! The configuration struct BaseSubSystemControllerConfig base_config_; + + //! Collection of flags which, if true, will cause the base class to make lifecycle service calls to managed nodes + // when ever the respective handle_on_ methods (ie. handle_on_configure) are called. + // by setting these flags to false an extending class chooses to implement that call itself. + // flags for on_shutdown and on_error are explicitly not provided since either should always result in subsystem shutdown. + // Overriding the respective methods without calling the base version will achieve the same external behavior but will also result in some internal variables not being populated. + // For correct behavior these flags should be set in the constructor + bool trigger_managed_nodes_configure_from_base_class_ = true; + bool trigger_managed_nodes_activate_from_base_class_ = true; + bool trigger_managed_nodes_deactivate_from_base_class_ = true; + bool trigger_managed_nodes_cleanup_from_base_class_ = true; }; } // namespace subsystem_controllers diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp index 3e0d331180..7f95c844eb 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp @@ -54,6 +54,9 @@ namespace subsystem_controllers //! Plugin manager to handle all the plugin specific discovery and reporting std::shared_ptr plugin_manager_; + //! Config for user provided parameters + GuidanceControllerConfig config_; + //! ROS handles cr2::SubPtr plugin_discovery_sub_; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp new file mode 100644 index 0000000000..220d540a26 --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp @@ -0,0 +1,64 @@ +#pragma once + +/* + * Copyright (C) 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 + +namespace subsystem_controllers +{ + /** + * \brief Stuct containing the algorithm configuration values for the GuidanceController + */ + struct GuidanceControllerConfig + + { + //! List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. + // Required plugins will be automatically activated at startup + // Required plugins cannot be deactivated by the user + std::vector required_plugins; + + //! List of guidance plugins which are not required but the user wishes to have automatically activated + // so that the user doesn't need to manually activate them via the UI on each launch (though they still can) + // this list should have zero intersection with the required_plugins + std::vector auto_activated_plugins; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const GuidanceControllerConfig &c) + { + + output << "GuidanceControllerConfig { " << std::endl + << "required_plugins: " << c.required_plugins << std::endl + << "auto_activated_plugins: " << c.auto_activated_plugins << std::endl + + << "required_plugins: [ " << std::endl; + + for (auto node : c.required_plugins) + output << node << " "; + + output << "] " << std::endl << "auto_activated_plugins: [ "; + + for (auto node : c.auto_activated_plugins) + output << node << " "; + + output << "] " << std::endl + << "}" << std::endl; + return output; + } + }; + +} // namespace subsystem_controllers diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index 6e854f168a..fdfa4b7c7d 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -39,25 +39,16 @@ namespace subsystem_controllers /** * \brief Constructor for PluginManager takes in require_plugin_names and service names */ - PluginManager(const std::vector& require_plugin_names, - const std::string& service_prefix, - const std::string& strategic_service_suffix, - const std::string& tactical_service_suffix); + PluginManager(const std::vector& required_plugins, + const std::vector& auto_activated_plugins); - /** - * \brief Get a list of registered plugins - */ - void get_registered_plugins(carma_planning_msgs::PluginListResponse& res); + void add_plugin(const std::string plugin); - /** - * \brief Get a list of active plugins - */ - void get_active_plugins(carma_planning_msgs::PluginListResponse& res); - - /** - * \brief Activate or deactivate a certain plugin - */ - bool activate_plugin(const std::string& name, const bool activate); + bool configure(std::vector plugins); + bool activate(std::vector plugins); + bool deactivate(std::vector plugins); + bool cleanup(std::vector plugins); + bool shutdown(std::vector plugins); /** * \brief Update the status of a certain plugin @@ -75,11 +66,11 @@ namespace subsystem_controllers bool get_tactical_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); private: + + //! Lifecycle Manager which will track the plugin nodes and call their lifecycle services on request + ros2_lifecycle_manager::Ros2LifecycleManager plugin_lifecycle_mgr_; - std::string service_prefix_; - std::string strategic_service_suffix_; - std::string tactical_service_suffix_; - EntryManager em_; + }; } \ No newline at end of file diff --git a/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp b/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp index 5a223c2829..3ed1682fff 100644 --- a/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp +++ b/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp @@ -143,6 +143,10 @@ namespace subsystem_controllers } + if (!trigger_managed_nodes_configure_from_base_class_) { + return CallbackReturn::SUCCESS; + } + // With all of our managed nodes now being tracked we can execute their configure operations bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); @@ -164,6 +168,10 @@ namespace subsystem_controllers { RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to activate"); + if (!trigger_managed_nodes_activate_from_base_class_) { + return CallbackReturn::SUCCESS; + } + bool success = lifecycle_mgr_.activate(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); if (success) @@ -184,6 +192,10 @@ namespace subsystem_controllers { RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to deactivate"); + if (!trigger_managed_nodes_deactivate_from_base_class_) { + return CallbackReturn::SUCCESS; + } + bool success = lifecycle_mgr_.deactivate(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); if (success) @@ -204,6 +216,10 @@ namespace subsystem_controllers { RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to cleanup"); + if (!trigger_managed_nodes_cleanup_from_base_class_) { + return CallbackReturn::SUCCESS; + } + bool success = lifecycle_mgr_.cleanup(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); if (success) diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 919b63e028..6cd1abe708 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -20,9 +20,11 @@ namespace subsystem_controllers { GuidanceControllerNode::GuidanceControllerNode(const rclcpp::NodeOptions &options) - : BaseSubsystemController(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; + {} cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &) { auto base_return = BaseSubsystemController::handle_on_configure(prev_state); @@ -32,6 +34,63 @@ namespace subsystem_controllers return base_return; } + config_ = Config(); + + auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes(); + + std::string plugin_namespace = base_config_.subsystem_namespace + "/plugins/" + + + std::vector guidance_plugin_nodes; + // Extract the nodes under the plugin namespaces (ie. /guidance/plugins/) + std::copy_if(base_managed_nodes.begin(), base_managed_nodes.end(), + std::back_inserter(guidance_plugin_nodes), + [](const std::string& s) { return s.rfind(plugin_namespace, 0) == 0; }); + + std::vector non_plugin_guidance_nodes = get_non_intersecting_set(base_managed_nodes, guidance_plugin_nodes); + + lifecycle_mgr_.set_managed_nodes(non_plugin_guidance_nodes); + + // Load required plugins and default enabled plugins + get_parameter>("required_plugins", config_.required_plugins); + get_parameter>("auto_activated_plugins", config_.auto_activated_plugins); + + + // The core need is that plugins need to be managed separately from guidance nodes + // Specifically every time a plugin is added it needs to be brought to the inactive state + // If the plugin is inactive it needs to be made active + plugin_lifecycle_mgr_.set_managed_nodes(); + + // Bringup method + // On deactivate we don't want to cause failure if its already deactivated + + + + // With all of our non-plugin managed nodes now being tracked we can execute their configure operations + bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(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; + } + + + // The core process for the guidance controller should be as follows + // On startup identify all nodes in the guidance namespace + // All nodes which are in guidance but NOT under /guidance/plugins will be managed by base_subsystem_controller + // All nodes under the /guidance/plugins/ namespace will be managed by the plugin manager + // - Only plugins listed as required or auto-start will be activated along with the controller + // - All other plugins will be identified via the plugin_discovery and tracked + + // TODO also in the system_controller or base_subsystem_controller we need to publish a system alert on startup failure so we can see the exception in the UI // TODO load parameters plugin_manager_ = std::make_shared(required_plugins_, plugin_service_prefix_, strategic_plugin_service_suffix_, tactical_plugin_service_suffix_); @@ -50,7 +109,7 @@ namespace subsystem_controllers activate_plugin_server_ = create_service( "plugins/get_active_plugins", - std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); // TODO this points to the wrong function get_strategic_plugin_by_capability_server_ = create_service( "plugins/get_strategic_plugin_by_capability", diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 72945666bb..81c9c1db08 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -19,16 +19,39 @@ namespace subsystem_controllers { - PluginManager::PluginManager(const std::vector& require_plugin_names, - const std::string& service_prefix, - const std::string& strategic_service_suffix, - const std::string& tactical_service_suffix) - : service_prefix_(service_prefix), - strategic_service_suffix_(strategic_service_suffix), - tactical_service_suffix_(tactical_service_suffix), - em_(EntryManager(require_plugin_names)) + PluginManager::PluginManager(const std::vector& required_plugins, + const std::vector& auto_activated_plugins, + ros2_lifecycle_manager::Ros2LifecycleManager plugin_lifecycle_mgr) // TODO should we pass pointer instead? + : required_plugins_(required_plugins), auto_activated_plugins_(auto_activated_plugins), plugin_lifecycle_mgr_(plugin_lifecycle_mgr_) {} + void add_plugin(const std::string plugin) + { + plugin_lifecycle_mgr_.add_managed_node(plugin); + // TODO need to bringup to inactive state here + } + + bool configure(std::vector plugins) + { + + } + bool activate(std::vector plugins) + { + + } + bool deactivate(std::vector plugins) + { + + } + bool cleanup(std::vector plugins) + { + + } + bool shutdown(std::vector plugins) + { + + } + void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginListResponse& res) { std::vector plugins = em_.get_entries(); From 025fc0ce30a9814ff2761a1e173f7b457dcff26c Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Thu, 2 Jun 2022 18:29:34 +0000 Subject: [PATCH 013/165] wip --- .../guidance_controller/entry.h | 4 +- .../guidance_controller/entry_manager.h | 17 +-- .../guidance_controller/plugin_manager.h | 15 ++- .../src/guidance_controller/entry.cpp | 24 ---- .../src/guidance_controller/entry_manager.cpp | 58 +++------ .../guidance_controller/plugin_manager.cpp | 116 ++++++++++++++---- 6 files changed, 124 insertions(+), 110 deletions(-) delete mode 100644 subsystem_controllers/src/guidance_controller/entry.cpp diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h index 09516fb40e..df0c4b27f8 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h @@ -26,10 +26,10 @@ namespace subsystem_controllers bool available_; bool active_; std::string name_; - 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); + Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability) + : available_(available), active_(active), name_(name), type_(type), capability_(capability) {} }; } \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h index 6ddd5fd6d9..bf42fca00b 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h @@ -18,6 +18,7 @@ #include #include +#include #include "entry.h" namespace subsystem_controllers @@ -30,10 +31,6 @@ namespace subsystem_controllers * \brief Default constructor for EntryManager. */ EntryManager(); - /*! - * \brief Constructor for EntryManager to set required entries. - */ - EntryManager(std::vector required_entries); /*! * \brief Add a new entry if the given name does not exist. @@ -56,18 +53,10 @@ namespace subsystem_controllers */ void delete_entry(const std::string& name); - /*! - * \brief Check if the entry is required - */ - bool is_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_; + // private map by entry name to keep track of all entries + std::unordered_map entry_map_; }; } \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index fdfa4b7c7d..374dc90190 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -27,6 +27,17 @@ namespace subsystem_controllers { + + struct Plugin + { + std::string name; + long timestamp; + std::string capability; + + Entry(const std::string& p_name, long p_timestamp, const std::string& p_capability) + : name(p_name), timestamp(p_timestamp), capability(p_capability) {} + }; + class PluginManager { public: @@ -40,7 +51,7 @@ namespace subsystem_controllers * \brief Constructor for PluginManager takes in require_plugin_names and service names */ PluginManager(const std::vector& required_plugins, - const std::vector& auto_activated_plugins); + const std::vector& auto_activated_plugins); // TODO take in pointe to lifecycle interface so we can unit test void add_plugin(const std::string plugin); @@ -69,6 +80,8 @@ namespace subsystem_controllers //! Lifecycle Manager which will track the plugin nodes and call their lifecycle services on request ros2_lifecycle_manager::Ros2LifecycleManager plugin_lifecycle_mgr_; + + std::unordered_map plugin_name_map_; diff --git a/subsystem_controllers/src/guidance_controller/entry.cpp b/subsystem_controllers/src/guidance_controller/entry.cpp deleted file mode 100644 index dce75170af..0000000000 --- a/subsystem_controllers/src/guidance_controller/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 subsystem_controllers -{ - 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/subsystem_controllers/src/guidance_controller/entry_manager.cpp b/subsystem_controllers/src/guidance_controller/entry_manager.cpp index 2257f3731f..a7af48736b 100644 --- a/subsystem_controllers/src/guidance_controller/entry_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/entry_manager.cpp @@ -22,66 +22,38 @@ namespace subsystem_controllers EntryManager::EntryManager() {} - EntryManager::EntryManager(std::vector required_entries):required_entries_(required_entries) {} - void EntryManager::update_entry(const Entry& entry) { - for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) - { - if(i->name_.compare(entry.name_) == 0) - { - // name and type of the entry wont change - i->active_ = entry.active_; - i->available_ = entry.available_; - i->timestamp_ = entry.timestamp_; - return; - } - } - entry_list_.push_back(entry); + entry_map_[entry.name_] = entry; } std::vector EntryManager::get_entries() const { - // returns the copy of the original list - return std::vector(entry_list_); + // returns the copy of the original data + std::vector entries; + entries.reserve(entry_map_.size()); + + for (const auto& e : entry_map_) + entries.push_back(e.second); + + return entries; } void EntryManager::delete_entry(const std::string& name) { - for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) - { - if(i->name_.compare(name) == 0) - { - entry_list_.erase(i); - return; - } - } + if (entry_map_.find(name) != entry_map_.end()) + entry_map_.erase(name); } boost::optional EntryManager::get_entry_by_name(const std::string& name) const { - for(auto i = entry_list_.begin(); i < entry_list_.end(); ++i) - { - if(i->name_.compare(name) == 0) - { - return *i; - } - } + if (entry_map_.find(name) != entry_map_.end()) + return entry_map_[name]; + + // use boost::optional because requested entry might not exist return boost::none; } - bool EntryManager::is_entry_required(const std::string& name) const - { - for(auto i = required_entries_.begin(); i < required_entries_.end(); ++i) - { - if(i->compare(name) == 0) - { - return true; - } - } - return false; - } - } diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 81c9c1db08..f7436bbffd 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -25,17 +25,74 @@ namespace subsystem_controllers : required_plugins_(required_plugins), auto_activated_plugins_(auto_activated_plugins), plugin_lifecycle_mgr_(plugin_lifecycle_mgr_) {} - void add_plugin(const std::string plugin) + + // Expected behavior + + void PluginManager::add_plugin(const Entry& plugin) { - plugin_lifecycle_mgr_.add_managed_node(plugin); - // TODO need to bringup to inactive state here + plugin_lifecycle_mgr_->add_managed_node(plugin.name); + + em_.update_entry(plugin); + + // TODO we don't actually have a current state, do we need a callback??? + // If this node is not in the active or inactive states then move the plugin to unconfigured + if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE + && get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + // Move plugin to unconfigured + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + // TODO it would be nice to handle this more eloquently where only failure of required plugins causes exceptions but I'm not sure the best way to do that yet + + + // TODO need to check if plugin is required + if (plugin is required) + { + throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to unconfigured state.") + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to configure newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!")); // TODO should we be tracking this so it doesn't get reset? + + Entry deactivated_entry = plugin; + deactivated_entry.active = false; + deactivated_entry.available = false; + em_.update_entry(deactivated_entry); + + return + } + + Entry deactivated_entry = plugin; + deactivated_entry.active = false; + em_.update_entry(deactivated_entry); + return; + } + + + // If the node is active or inactive then + // when adding a plugin it should be brought to the inactive state + // We do not need transition it beyond inactive in this function as that will be managed by the plugin activation process via UI or parameters + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + // TODO it would be nice to handle this more eloquently where only failure of required plugins causes exceptions but I'm not sure the best way to do that yet + throw std::runtime_error("Newly discovered plugin " + plugin.name + " could not be brought to inactive state.") + } + } - bool configure(std::vector plugins) + bool configure() { - + std::vector plugins_to_transition; + plugins_to_transition.reserve(required_plugins_.size() + auto_activated_plugins_.size()); + + } - bool activate(std::vector plugins) + bool activate() { } @@ -52,11 +109,11 @@ namespace subsystem_controllers } - void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginListResponse& res) + void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginListRequest&, carma_planning_msgs::srv::PluginListResponse& res) { std::vector plugins = em_.get_entries(); // convert to plugin list - for(auto i = plugins.begin(); i < plugins.end(); ++i) + for(const auto& plugin : plugin_map_) { carma_planning_msgs::msg::Plugin plugin; plugin.activated = i->active_; @@ -67,7 +124,7 @@ namespace subsystem_controllers } } - void PluginManager::get_active_plugins(carma_planning_msgs::srv::PluginListResponse& res) + void PluginManager::get_active_plugins(carma_planning_msgs::srv::PluginListRequest&, carma_planning_msgs::srv::PluginListResponse& res) { std::vector plugins = em_.get_entries(); // convert to plugin list @@ -76,7 +133,7 @@ namespace subsystem_controllers if(i->active_) { carma_planning_msgs::msg::Plugin plugin; - plugin.activated = true; + plugin.activated = i->active_; plugin.available = i->available_; plugin.name = i->name_; plugin.type = i->type_; @@ -85,33 +142,40 @@ namespace subsystem_controllers } } - bool PluginManager::activate_plugin(const std::string& name, const bool activate) + void PluginManager::activate_plugin(cav_srvs::PluginActivationRequest& req, cav_srvs::PluginActivationResponse& res) { - boost::optional requested_plugin = em_.get_entry_by_name(name); - if(requested_plugin) + boost::optional requested_plugin = em_.get_entry_by_name(req.plugin_name); + + if(!requested_plugin) // If not a plugin then obviously not activated. Though really it would be better to have an indication of name failure in the message { - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry updated_entry(requested_plugin->available_, activate, requested_plugin->name_, 0, requested_plugin->type_, requested_plugin->capability_); - em_.update_entry(updated_entry); - return true; + res.newstate = false; + return; } - return false; + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, req.plugin_name); + + bool activated = (result_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + Entry updated_entry(requested_plugin->available_, activated, requested_plugin->name_, requested_plugin->type_, requested_plugin->capability_); + em_.update_entry(updated_entry); + + res.newstate = activated; } void PluginManager::update_plugin_status(const carma_planning_msgs::msg::PluginConstPtr& msg) { ROS_DEBUG_STREAM("received status from: " << msg->name); boost::optional requested_plugin = em_.get_entry_by_name(msg->name); - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry plugin(msg->available, msg->activated, msg->name, 0, msg->type, msg->capability); - // if it already exists, we do not change its activation status - if(requested_plugin) - { - plugin.active_ = requested_plugin->active_; - } else if(em_.is_entry_required(msg->name)) + + bool activation = msg->activated; + if (!requested_plugin) // This is a new plugin so we need to add it { - plugin.active_ = true; + Entry plugin(msg->available_, activated, msg->name_, msg->type_, msg->capability_); + add_plugin(plugin); } + + Entry plugin(msg->available, activation, msg->name, msg->type, msg->capability); + em_.update_entry(plugin); } From 45da993f2a122a5251948a0feaeadcf5e85426cf Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Thu, 2 Jun 2022 21:07:09 +0000 Subject: [PATCH 014/165] WIP --- .../guidance_controller/plugin_manager.h | 3 +- .../guidance_controller/plugin_manager.cpp | 77 +++++++++++++------ 2 files changed, 56 insertions(+), 24 deletions(-) diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index 374dc90190..ef636cfdca 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -81,7 +81,8 @@ namespace subsystem_controllers //! Lifecycle Manager which will track the plugin nodes and call their lifecycle services on request ros2_lifecycle_manager::Ros2LifecycleManager plugin_lifecycle_mgr_; - std::unordered_map plugin_name_map_; + std::unordered_set required_plugins_; + std::unordered_set auto_activated_plugins_; diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index f7436bbffd..be449209de 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -22,7 +22,9 @@ namespace subsystem_controllers PluginManager::PluginManager(const std::vector& required_plugins, const std::vector& auto_activated_plugins, ros2_lifecycle_manager::Ros2LifecycleManager plugin_lifecycle_mgr) // TODO should we pass pointer instead? - : required_plugins_(required_plugins), auto_activated_plugins_(auto_activated_plugins), plugin_lifecycle_mgr_(plugin_lifecycle_mgr_) + : required_plugins_(required_plugins.begin(), required_plugins_.end()), + auto_activated_plugins_(auto_activated_plugins.begin(), auto_activated_plugins.end()), + plugin_lifecycle_mgr_(plugin_lifecycle_mgr_) {} @@ -34,61 +36,90 @@ namespace subsystem_controllers em_.update_entry(plugin); + Entry deactivated_entry = plugin; + + // TODO we don't actually have a current state, do we need a callback??? // If this node is not in the active or inactive states then move the plugin to unconfigured if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + + deactivated_entry.active_ = false; + // Move plugin to unconfigured auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - // TODO it would be nice to handle this more eloquently where only failure of required plugins causes exceptions but I'm not sure the best way to do that yet - + { - // TODO need to check if plugin is required - if (plugin is required) + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name) != required_plugins_.end()) { throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to unconfigured state.") } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to configure newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!")); // TODO should we be tracking this so it doesn't get reset? + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to cleanup newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!")); - Entry deactivated_entry = plugin; - deactivated_entry.active = false; - deactivated_entry.available = false; - em_.update_entry(deactivated_entry); + deactivated_entry.available_ = false; - return } - Entry deactivated_entry = plugin; - deactivated_entry.active = false; em_.update_entry(deactivated_entry); return; } - // If the node is active or inactive then - // when adding a plugin it should be brought to the inactive state + // If the node is active or inactive then, + // when adding a plugin, it should be brought to the inactive state // We do not need transition it beyond inactive in this function as that will be managed by the plugin activation process via UI or parameters auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - // TODO it would be nice to handle this more eloquently where only failure of required plugins causes exceptions but I'm not sure the best way to do that yet - throw std::runtime_error("Newly discovered plugin " + plugin.name + " could not be brought to inactive state.") + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name) != required_plugins_.end()) + { + throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to inactive state.") + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to configure newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!")); + + deactivated_entry.available_ = false; } - + + deactivated_entry.active = false; + em_.update_entry(deactivated_entry); + } bool configure() { std::vector plugins_to_transition; plugins_to_transition.reserve(required_plugins_.size() + auto_activated_plugins_.size()); + plugins_to_transition.insert(plugins_to_transition.begin(), required_plugins_.begin(), required_plugins_.end()); + plugins_to_transition.insert(plugins_to_transition.begin(), auto_activated_plugins_.begin(), auto_activated_plugins_.end()); + + + auto failed_plugins = plugin_lifecycle_mgr_->configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms), false, plugins_to_transition); + + for (const auto& p : failed_plugins) + { + if (required_plugins_.find(p) != required_plugins_.end()) + { + throw std::runtime_error("Failed to trigger configure transition for required plugin: " + p); + } + + if (auto_activated_plugins_.find(p) != auto_activated_plugins_.end() + && ) + { + + } + } } @@ -167,14 +198,14 @@ namespace subsystem_controllers ROS_DEBUG_STREAM("received status from: " << msg->name); boost::optional requested_plugin = em_.get_entry_by_name(msg->name); - bool activation = msg->activated; if (!requested_plugin) // This is a new plugin so we need to add it { - Entry plugin(msg->available_, activated, msg->name_, msg->type_, msg->capability_); + Entry plugin(msg->available_, msg->activated_, msg->name_, msg->type_, msg->capability_); add_plugin(plugin); + return; } - Entry plugin(msg->available, activation, msg->name, msg->type, msg->capability); + Entry plugin(msg->available, msg->activated_, msg->name, msg->type, msg->capability); em_.update_entry(plugin); } From a5c36fb959b35009096a2f92b1e7d281c6d35f38 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Tue, 7 Jun 2022 14:37:27 +0000 Subject: [PATCH 015/165] Fix typos in readme --- carma_wm_ctrl/README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/carma_wm_ctrl/README.md b/carma_wm_ctrl/README.md index 230a07eea4..1db3c2a183 100644 --- a/carma_wm_ctrl/README.md +++ b/carma_wm_ctrl/README.md @@ -1,5 +1,7 @@ # carma_wm_ctrl This packages provides logic for updating a [carma_wm](../carma_wm) compatable lanelet2 map at runtime using [CARMACloud](https://github.com/usdot-fhwa-stol/carma-cloud) geofences. -The carma_wm_broadcaster node sits between the lanelet2 map loader and the rest of the CARMAPlatform system. When a new geofence is recieved, the map is updated and the update is communicated to the rest of the CARMAPlatform. -If a base map is recieved which does not contain the carma_wm compatable regulatory elements, the carma_wm_broadcaster node will make an initial best effort attempt to make the map compatable. There is no guarenetee that this will work so starting with a compatible map is always recommended. +The carma_wm_broadcaster node sits between the lanelet2 map loader and the rest of the CARMAPlatform system. When a new geofence is received, the map is updated and the update is communicated to the rest of the CARMAPlatform. +If a base map is received which does not contain the carma_wm compatible regulatory elements, the carma_wm_broadcaster node will make an initial best effort attempt to make the map compatible. There is no guarantee that this will work so starting with a compatible map is always recommended. + +Design documentation for this package can be found in the carma_wm_ctrl section of [this confluence page.](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/1324122268/Detailed+Design+-+World+Model+Road+Network+Library) From 13418599540088adafe540b17327b383cdcc65ad Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Wed, 8 Jun 2022 14:57:51 +0000 Subject: [PATCH 016/165] Modify plugin base class to use fully specified node name instead of user provided name in plugin_discovery --- .../carma_guidance_plugins/plugin_base_node.hpp | 10 +--------- carma_guidance_plugins/src/control_plugin.cpp | 2 +- carma_guidance_plugins/src/plugin_base_node.cpp | 2 +- carma_guidance_plugins/src/strategic_plugin.cpp | 2 +- carma_guidance_plugins/src/tactical_plugin.cpp | 2 +- carma_guidance_plugins/test/TestPlugins.h | 12 ------------ carma_guidance_plugins/test/node_test.cpp | 6 +++--- .../template_control_plugin_node.hpp | 2 -- .../template_strategic_plugin_node.hpp | 2 -- .../template_tactical_plugin_node.hpp | 2 -- .../src/template_control_plugin_node.cpp | 4 ---- .../src/template_strategic_plugin_node.cpp | 4 ---- .../src/template_tactical_plugin_node.cpp | 4 ---- 13 files changed, 8 insertions(+), 46 deletions(-) diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp index 72dac6ce1d..da216637bf 100644 --- a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp +++ b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp @@ -32,7 +32,7 @@ namespace carma_guidance_plugins * This includes basic state machine management (largely delegated to ROS2 lifecycle behavior), required interfaces, and plugin discovery * * Extending classes must implement the on_configure_plugin method to load parameters, and my override the other state transitions methods on__plugin if desired. - * Additionally, extending classes must implement the methods such as get_plugin_name() which are used to populate the plugin discovery message. + * Additionally, extending classes must implement the methods such as get_version_id() which are used to populate the plugin discovery message. * * NOTE: At the moment, this class and all extending classes are setup to support only single threading. * @@ -132,14 +132,6 @@ namespace carma_guidance_plugins */ virtual std::string get_capability() = 0; - /** - * \brief Returns the name of this plugin. - * This name may be automatically prepended to plugin API service or topic names. - * - * \return Name of this plugin - */ - virtual std::string get_plugin_name() = 0; - /** * \brief Returns the version id of this plugin. * diff --git a/carma_guidance_plugins/src/control_plugin.cpp b/carma_guidance_plugins/src/control_plugin.cpp index 401a748be6..0d6dd7f8d4 100644 --- a/carma_guidance_plugins/src/control_plugin.cpp +++ b/carma_guidance_plugins/src/control_plugin.cpp @@ -63,7 +63,7 @@ namespace carma_guidance_plugins current_velocity_sub_ = create_subscription("vehicle/twist", 1, std::bind(&ControlPlugin::current_twist_callback, this, std_ph::_1)); - trajectory_plan_sub_ = create_subscription(get_plugin_name() + "/plan_trajectory", 1, + trajectory_plan_sub_ = create_subscription(std::string(get_name()) + "/plan_trajectory", 1, std::bind(&ControlPlugin::current_trajectory_callback, this, std_ph::_1)); vehicle_cmd_pub_ = create_publisher("ctrl_raw", 1); diff --git a/carma_guidance_plugins/src/plugin_base_node.cpp b/carma_guidance_plugins/src/plugin_base_node.cpp index 358a3ed835..22f171df7d 100644 --- a/carma_guidance_plugins/src/plugin_base_node.cpp +++ b/carma_guidance_plugins/src/plugin_base_node.cpp @@ -142,7 +142,7 @@ namespace carma_guidance_plugins } carma_planning_msgs::msg::Plugin msg; - msg.name = get_plugin_name(); + msg.name = std::string(get_namespace()) + std::string(get_name()); msg.version_id = get_version_id(); msg.type = get_type(); msg.available = get_availability(); diff --git a/carma_guidance_plugins/src/strategic_plugin.cpp b/carma_guidance_plugins/src/strategic_plugin.cpp index 9eb2868af8..1d8016bfa4 100644 --- a/carma_guidance_plugins/src/strategic_plugin.cpp +++ b/carma_guidance_plugins/src/strategic_plugin.cpp @@ -38,7 +38,7 @@ namespace carma_guidance_plugins carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { // Initialize plan maneuvers service - plan_maneuvers_service_ = create_service(get_plugin_name() + "/plan_maneuvers", + plan_maneuvers_service_ = create_service(std::string(get_name()) + "/plan_maneuvers", [this] (auto header, auto req, auto resp) { if (this->get_activation_status()) // Only trigger when activated { diff --git a/carma_guidance_plugins/src/tactical_plugin.cpp b/carma_guidance_plugins/src/tactical_plugin.cpp index 024e3250e6..94fe88737b 100644 --- a/carma_guidance_plugins/src/tactical_plugin.cpp +++ b/carma_guidance_plugins/src/tactical_plugin.cpp @@ -38,7 +38,7 @@ namespace carma_guidance_plugins carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { // Initialize plan trajectory service - plan_trajectory_service_ = create_service(get_plugin_name() + "/plan_trajectory", + plan_trajectory_service_ = create_service(std::string(get_name()) + "/plan_trajectory", [this] (auto header, auto req, auto resp) { if (this->get_activation_status()) // Only trigger when activated { diff --git a/carma_guidance_plugins/test/TestPlugins.h b/carma_guidance_plugins/test/TestPlugins.h index 5a8b0daf29..6a35cfaea5 100644 --- a/carma_guidance_plugins/test/TestPlugins.h +++ b/carma_guidance_plugins/test/TestPlugins.h @@ -43,10 +43,6 @@ namespace carma_guidance_plugins return true; } - std::string get_plugin_name() override { - return "TestStrategicPlugin"; - } - std::string get_version_id() override { return "1.0"; } @@ -84,10 +80,6 @@ namespace carma_guidance_plugins return true; } - std::string get_plugin_name() override { - return "TestTacticalPlugin"; - } - std::string get_version_id() override { return "1.1"; } @@ -123,10 +115,6 @@ namespace carma_guidance_plugins return true; } - std::string get_plugin_name() override { - return "TestControlPlugin"; - } - std::string get_version_id() override { return "1.2"; } diff --git a/carma_guidance_plugins/test/node_test.cpp b/carma_guidance_plugins/test/node_test.cpp index 0beb9e7659..7507f09c7e 100644 --- a/carma_guidance_plugins/test/node_test.cpp +++ b/carma_guidance_plugins/test/node_test.cpp @@ -95,9 +95,9 @@ TEST(carma_guidance_plugins_test, connections_test) { ASSERT_TRUE(tactical_plugin->get_availability()); ASSERT_TRUE(control_plugin->get_availability()); - ASSERT_EQ(strategic_plugin->get_plugin_name(), "TestStrategicPlugin"); - ASSERT_EQ(tactical_plugin->get_plugin_name(), "TestTacticalPlugin"); - ASSERT_EQ(control_plugin->get_plugin_name(), "TestControlPlugin"); + ASSERT_EQ(strategic_plugin->get_name(), "strategic_plugin_test"); + ASSERT_EQ(tactical_plugin->get_name(), "tactical_plugin_test"); + ASSERT_EQ(control_plugin->get_name(), "control_plugin_test"); ASSERT_EQ(strategic_plugin->get_version_id(), "1.0"); ASSERT_EQ(tactical_plugin->get_version_id(), "1.1"); diff --git a/template_package/include/template_package/template_control_plugin_node.hpp b/template_package/include/template_package/template_control_plugin_node.hpp index 8ded54b871..fdd5c3f84a 100644 --- a/template_package/include/template_package/template_control_plugin_node.hpp +++ b/template_package/include/template_package/template_control_plugin_node.hpp @@ -89,8 +89,6 @@ namespace bool get_availability() override; - std::string get_plugin_name() override; - std::string get_version_id() override; /** diff --git a/template_package/include/template_package/template_strategic_plugin_node.hpp b/template_package/include/template_package/template_strategic_plugin_node.hpp index 4648115165..abaf36a7b6 100644 --- a/template_package/include/template_package/template_strategic_plugin_node.hpp +++ b/template_package/include/template_package/template_strategic_plugin_node.hpp @@ -92,8 +92,6 @@ namespace bool get_availability() override; - std::string get_plugin_name() override; - std::string get_version_id() override; /** diff --git a/template_package/include/template_package/template_tactical_plugin_node.hpp b/template_package/include/template_package/template_tactical_plugin_node.hpp index 546a5bef27..4c9c1b8780 100644 --- a/template_package/include/template_package/template_tactical_plugin_node.hpp +++ b/template_package/include/template_package/template_tactical_plugin_node.hpp @@ -92,8 +92,6 @@ namespace bool get_availability() override; - std::string get_plugin_name() override; - std::string get_version_id() override; /** diff --git a/template_package/src/template_control_plugin_node.cpp b/template_package/src/template_control_plugin_node.cpp index 2a3ad09bf4..f45a88df6e 100644 --- a/template_package/src/template_control_plugin_node.cpp +++ b/template_package/src/template_control_plugin_node.cpp @@ -110,10 +110,6 @@ namespace return true; // TODO for user implement actual check on availability if applicable to plugin } - std::string Node::get_plugin_name() { - return "TODO for user specify plugin name here"; - } - std::string Node::get_version_id() { return "TODO for user specify plugin version id here"; } diff --git a/template_package/src/template_strategic_plugin_node.cpp b/template_package/src/template_strategic_plugin_node.cpp index cd2d288c48..c2299a79e4 100644 --- a/template_package/src/template_strategic_plugin_node.cpp +++ b/template_package/src/template_strategic_plugin_node.cpp @@ -111,10 +111,6 @@ namespace return true; // TODO for user implement actual check on availability if applicable to plugin } - std::string Node::get_plugin_name() { - return "TODO for user specify plugin name here"; - } - std::string Node::get_version_id() { return "TODO for user specify plugin version id here"; } diff --git a/template_package/src/template_tactical_plugin_node.cpp b/template_package/src/template_tactical_plugin_node.cpp index 7eb48f16ed..e02ad3e4db 100644 --- a/template_package/src/template_tactical_plugin_node.cpp +++ b/template_package/src/template_tactical_plugin_node.cpp @@ -111,10 +111,6 @@ namespace return true; // TODO for user implement actual check on availability if applicable to plugin } - std::string Node::get_plugin_name() { - return "TODO for user specify plugin name here"; - } - std::string Node::get_version_id() { return "TODO for user specify plugin version id here"; } From c57bd0db65dcc57f80d3b0e17bb79622e0c7ac85 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Wed, 8 Jun 2022 21:14:24 +0000 Subject: [PATCH 017/165] basic logic and structure completed --- .../guidance_controller/entry.h | 20 +- .../guidance_controller/entry_manager.h | 11 +- .../guidance_controller.hpp | 4 +- .../guidance_controller/plugin_manager.h | 121 +++++--- .../guidance_controller.cpp | 104 ++++--- .../guidance_controller/plugin_manager.cpp | 268 +++++++++++++++--- 6 files changed, 394 insertions(+), 134 deletions(-) diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h index df0c4b27f8..92d77edc4e 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -20,16 +20,28 @@ namespace subsystem_controllers { + /** + * \brief An entry represents a plugins details for the purposes of tracking + */ struct Entry { - /* data */ + //! Availability flag of a plugin bool available_; + //! Activation flag of a plugin bool active_; + //! Fully specified node name of a plugin std::string name_; + //! Type of the plugin from the message enum in carma_planning_msgs::Plugin uint8_t type_; + //! The capability string of the plugin std::string capability_; + //! Flag indicating if the user requested this plugin be activated + bool user_requested_activation_; - Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability) - : available_(available), active_(active), name_(name), type_(type), capability_(capability) {} + /** + * \brief All fields constructor + */ + Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability, bool user_requested_activation) + : available_(available), active_(active), name_(name), type_(type), capability_(capability), user_requested_activation_(user_requested_activation) {} }; } \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h index bf42fca00b..1dc4264406 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h @@ -23,6 +23,9 @@ namespace subsystem_controllers { + /** + * \brief An entry manager keeps track of the set of entries and makes it easy to add or remove entries + */ class EntryManager { public: @@ -34,12 +37,14 @@ namespace subsystem_controllers /*! * \brief Add a new entry if the given name does not exist. - * Update an existing entry if the given name exists. + * Update an existing entry if the given name exists. + * + * \param entry The entry to update or add */ void update_entry(const Entry& entry); /*! - * \brief Get all registed entries as a list. + * \brief Get all entries as a list. */ std::vector get_entries() const; @@ -55,7 +60,7 @@ namespace subsystem_controllers private: - // private map by entry name to keep track of all entries + //! private map by entry name to keep track of all entries std::unordered_map entry_map_; }; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp index 7f95c844eb..404b023f12 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp @@ -69,9 +69,9 @@ namespace subsystem_controllers cr2::ServicePtr get_strategic_plugin_by_capability_server_; - cr2::ServicePtrget_tactical_plugin_by_capability_server_; + cr2::ServicePtr get_tactical_plugin_by_capability_server_; - std::unordered_set auto_activated_plugins_; + cr2::ServicePtr get_control_plugin_by_capability_server_; }; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index ef636cfdca..da85b178d8 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2019-2021 LEIDOS. + * Copyright (C) 2019-2022LEIDOS. * * 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 @@ -20,71 +20,126 @@ #include #include #include +#include #include "entry_manager.h" -#include -#include +#include "entry.h" namespace subsystem_controllers { - struct Plugin - { - std::string name; - long timestamp; - std::string capability; - - Entry(const std::string& p_name, long p_timestamp, const std::string& p_capability) - : name(p_name), timestamp(p_timestamp), capability(p_capability) {} - }; + using GetParentNodeStateFunc = std::function; + /** + * \brief The PluginManager serves as a component to manage CARMA Guidance Plugins via their ros2 lifecycle interfaces + */ class PluginManager { public: - - /** - * \brief Default constructor for PluginManager - */ - PluginManager() = default; + + // TODO do we need a system alert callback in this manager as well /** - * \brief Constructor for PluginManager takes in require_plugin_names and service names + * \brief Constructor for PluginManager + * + * \param required_plugins The set of plugins which will be treated as required. A failure in these plugins will result in an exception + * \param auto_activated_plugins The set of plugins which will be automatically activated at first system activation but not treated specially after that. + * \param plugin_lifecycle_mgr A fully initialized lifecycle manager which will be used trigger plugin transitions + * \param get_parent_state_func A callback which will allow this object to access the parent process lifecycle state */ PluginManager(const std::vector& required_plugins, - const std::vector& auto_activated_plugins); // TODO take in pointe to lifecycle interface so we can unit test - - void add_plugin(const std::string plugin); + const std::vector& auto_activated_plugins, + std::shared_ptr plugin_lifecycle_mgr, + GetParentNodeStateFunc get_parent_state_func); - bool configure(std::vector plugins); - bool activate(std::vector plugins); - bool deactivate(std::vector plugins); - bool cleanup(std::vector plugins); - bool shutdown(std::vector plugins); + /** + * Below are the state transition methods which will cause this manager to trigger the corresponding + * state transitions in the managed plugins. + */ + void configure(); + void activate(); + void deactivate(); + void cleanup(); + void shutdown(); /** * \brief Update the status of a certain plugin + * + * \param msg A plugin status message */ void update_plugin_status(const carma_planning_msgs::PluginConstPtr& msg); /** * \brief Get strategic plugins by capability + * + * \param req The req which identifies which capability is required + * \param res The res which identifies the strategic plugins with the requested capability */ - bool get_strategic_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); + void get_strategic_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); /** * \brief Get tactical plugins by capability + * + * \param req The req which identifies which capability is required + * \param res The res which identifies the tactical plugins with the requested capability + */ + void get_tactical_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); + + /** + * \brief Get control plugins by capability + * + * \param req The req which identifies which capability is required + * \param res The res which identifies the control plugins with the requested capability */ - bool get_tactical_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); + void get_control_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); - private: + protected: - //! Lifecycle Manager which will track the plugin nodes and call their lifecycle services on request - ros2_lifecycle_manager::Ros2LifecycleManager plugin_lifecycle_mgr_; + /** + * \brief Add the specified entry to our plugin management + * This function will attempt to move the newly detected plugin to the required state + * based on this nodes own state + * + * \param plugin The entry representing the plugin to add + * + * \throws std::runtime_error if this was a required plugin and it could not be transitioned as needed + */ + void add_plugin(const Entry& plugin); + /** + * \brief Returns true if the provided base capability hierarchy can achieve the requested capability hierarchy + * A capability hierarchy is described as a list of strings where the first string is the most generic description + * of the capability while the last string the is the most detailed description. + * + * For example the capability "tactical_plan/plan_trajectory" would be described as ["tactical_plan", "plan_trajectory"] + * if the user wanted to match this to "tactical_plan/plan_trajectory/platooning_trajectory" then the compared_capability_levels + * would be ["tactical_plan", "plan_trajectory", "compared_capability_levels"] + * Matching those two inputs as ( ["tactical_plan", "plan_trajectory"], ["tactical_plan", "plan_trajectory", "compared_capability_levels"]) + * would be false because the compared_capability_levels is more detailed than the base. + * By contrast, matching ( ["tactical_plan", "plan_trajectory", "compared_capability_levels"], ["tactical_plan", "plan_trajectory"] ) + * would be true since the base is more generic then the request + * + * // TODO check with Kyle on this because it maybe should be the reverse since a more specific capability may require specific meta data. + * + * \param base_capability_levels The base hierarchy to check for compatability + * \param compared_capability_levels The compared_capability_levels which are being checked against the base + * + * \return True if base_capability_levels supports compared_capability_levels + */ + bool matching_capability(const std::vector& base_capability_levels, const std::vector& compared_capability_levels) + + //! Set of required plugins a failure of which necessitates system shutdown std::unordered_set required_plugins_; + + //! Set of use specified auto activated plugins which will automatically started without need for user input + // These will only be activated once, if the user later deactivates them then that behavior will be preserved std::unordered_set auto_activated_plugins_; - - + + //! Lifecycle Manager which will track the plugin nodes and call their lifecycle services on request + std::shared_ptr plugin_lifecycle_mgr_; + + //! Callback to retrieve the lifecycle state of the parent process + GetParentNodeStateFunc get_parent_state_func_; }; } \ No newline at end of file diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 6cd1abe708..8b3628560a 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -55,45 +55,13 @@ namespace subsystem_controllers get_parameter>("required_plugins", config_.required_plugins); get_parameter>("auto_activated_plugins", config_.auto_activated_plugins); - - // The core need is that plugins need to be managed separately from guidance nodes - // Specifically every time a plugin is added it needs to be brought to the inactive state - // If the plugin is inactive it needs to be made active - plugin_lifecycle_mgr_.set_managed_nodes(); - - // Bringup method - // On deactivate we don't want to cause failure if its already deactivated - + RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_); + // The core need is that plugins need to be managed separately from guidance nodes + auto plugin_lifecycle_manager = std::make_shared( + get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface()); - // With all of our non-plugin managed nodes now being tracked we can execute their configure operations - bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(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; - } - - - // The core process for the guidance controller should be as follows - // On startup identify all nodes in the guidance namespace - // All nodes which are in guidance but NOT under /guidance/plugins will be managed by base_subsystem_controller - // All nodes under the /guidance/plugins/ namespace will be managed by the plugin manager - // - Only plugins listed as required or auto-start will be activated along with the controller - // - All other plugins will be identified via the plugin_discovery and tracked - - // TODO also in the system_controller or base_subsystem_controller we need to publish a system alert on startup failure so we can see the exception in the UI - // TODO load parameters - - plugin_manager_ = std::make_shared(required_plugins_, plugin_service_prefix_, strategic_plugin_service_suffix_, tactical_plugin_service_suffix_); + plugin_manager_ = std::make_shared(config_.required_plugins, config_.auto_activated_plugins, plugin_lifecycle_manager, [this](){ return get_current_state().id(); }); plugin_discovery_sub_ = create_subscription( "plugin_discovery", 50, @@ -108,8 +76,8 @@ namespace subsystem_controllers std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); activate_plugin_server_ = create_service( - "plugins/get_active_plugins", - std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); // TODO this points to the wrong function + "plugins/activate_plugin", // TODO check topic name + std::bind(&PluginManager::activate_plugin, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); get_strategic_plugin_by_capability_server_ = create_service( "plugins/get_strategic_plugin_by_capability", @@ -119,6 +87,31 @@ namespace subsystem_controllers "plugins/get_tactical_plugin_by_capability", std::bind(&PluginManager::get_tactical_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + get_control_plugin_by_capability_server_ = create_service( + "plugins/get_control_plugin_by_capability", + std::bind(&PluginManager::get_control_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + + + + // With all of our non-plugin managed nodes now being tracked we can execute their configure operations + bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); + + // Configure our plugins + plugin_manager_->configure(); // TODO callback returns for this + + + 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; + } return cr2::CallbackReturn::SUCCESS; } @@ -132,11 +125,7 @@ namespace subsystem_controllers return base_return; } - // In activate we want to activate all plugins which are required if they haven't already been activated by base class - // We want to skip all plugins which are not required - // TODO it seems like we may want some sort of start activated configuration for plugins as well - - plugin_manager_-> + plugin_manager_->activate(); // TODO callback return } @@ -149,8 +138,31 @@ namespace subsystem_controllers return base_return; } - // Iterate over all plugins to deactivate them - plugin_manager_.get_registered_plugins(); + plugin_manager_->deactivate(); // TODO callback return + } + + cr2::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) + { + auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not deactivate"); + return base_return; + } + + plugin_manager_->cleanup(); // TODO callback return + } + + cr2::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) + { + auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not deactivate"); + return base_return; + } + + plugin_manager_->shutdown(); // TODO callback return } diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index be449209de..9c7d89625e 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -14,6 +14,7 @@ * the License. */ +#include #include "plugin_manager.h" namespace subsystem_controllers @@ -21,14 +22,28 @@ namespace subsystem_controllers PluginManager::PluginManager(const std::vector& required_plugins, const std::vector& auto_activated_plugins, - ros2_lifecycle_manager::Ros2LifecycleManager plugin_lifecycle_mgr) // TODO should we pass pointer instead? + std::shared_ptr plugin_lifecycle_mgr, + GetParentNodeStateFunc get_parent_state_func) // TODO should we pass pointer instead? : required_plugins_(required_plugins.begin(), required_plugins_.end()), auto_activated_plugins_(auto_activated_plugins.begin(), auto_activated_plugins.end()), - plugin_lifecycle_mgr_(plugin_lifecycle_mgr_) - {} + plugin_lifecycle_mgr_(plugin_lifecycle_mgr_), get_parent_state_func_(get_parent_state_func) + { + // For all required and auto activated plugins add unknown entries but with + // user_requested_activation set to true. + // This will be used later to determine how to transition the plugin specified by that entry + for (const auto& p : required_plugins_) { + Entry e(false, false, p.name, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + em_.update_entry(e); + plugin_lifecycle_mgr_->add_managed_node(p.name); + } + for (const auto& p : auto_activated_plugins_) { + Entry e(false, false, p.name, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + em_.update_entry(e); + plugin_lifecycle_mgr_->add_managed_node(p.name); + } - // Expected behavior + } void PluginManager::add_plugin(const Entry& plugin) { @@ -38,29 +53,29 @@ namespace subsystem_controllers Entry deactivated_entry = plugin; + auto parent_node_state = get_parent_state_func(); - // TODO we don't actually have a current state, do we need a callback??? // If this node is not in the active or inactive states then move the plugin to unconfigured - if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE - && get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + if (parent_node_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE + && parent_node_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { deactivated_entry.active_ = false; - // Move plugin to unconfigured - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name); + // Move plugin to match this nodes state + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(parent_node_state, plugin.name); - if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + if(result_state != parent_node_state) { // If this plugin was required then trigger exception if (required_plugins_.find(plugin.name) != required_plugins_.end()) { - throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to unconfigured state.") + throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to controller node state."); } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to cleanup newly discovered non-required plugin: " + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to transition newly discovered non-required plugin: " << plugin.name << " Marking as deactivated and unavailable!")); deactivated_entry.available_ = false; @@ -82,7 +97,7 @@ namespace subsystem_controllers // If this plugin was required then trigger exception if (required_plugins_.find(plugin.name) != required_plugins_.end()) { - throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to inactive state.") + throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to inactive state."); } // If this plugin was not required log an error and mark it is unavailable and deactivated @@ -97,47 +112,156 @@ namespace subsystem_controllers } - bool configure() + void configure() { - std::vector plugins_to_transition; - plugins_to_transition.reserve(required_plugins_.size() + auto_activated_plugins_.size()); - plugins_to_transition.insert(plugins_to_transition.begin(), required_plugins_.begin(), required_plugins_.end()); - plugins_to_transition.insert(plugins_to_transition.begin(), auto_activated_plugins_.begin(), auto_activated_plugins_.end()); + + // Bring all known plugins to the inactive state + for (auto plugin : em_.get_entries()) + { + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_); + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be configured."); + } - auto failed_plugins = plugin_lifecycle_mgr_->configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms), false, plugins_to_transition); + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to configure newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!")); - for (const auto& p : failed_plugins) + Entry deactivated_entry = plugin; + deactivated_entry.active = false; + deactivated_entry.available = false; + deactivated_entry.user_requested_activation_ = false; // TODO is there an edge case where the user hits this first???? + em_.update_entry(deactivated_entry); + } + + } + + } + + void activate() + { + // Bring all required or auto activated plugins to the active state + for (auto plugin : em_.get_entries()) { - if (required_plugins_.find(p) != required_plugins_.end()) + // If this is not a plugin slated for activation then continue and leave up to user to activate manually later + if (!plugin.user_requested_activation_) + continue; + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, plugin.name_); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - throw std::runtime_error("Failed to trigger configure transition for required plugin: " + p); + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be activated."); + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to activate newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!")); + + Entry deactivated_entry = plugin; + deactivated_entry.active = false; + deactivated_entry.available = false; + deactivated_entry.user_requested_activation_ = false; + em_.update_entry(deactivated_entry); } - if (auto_activated_plugins_.find(p) != auto_activated_plugins_.end() - && ) + // If this was an auto activated plugin and not required then we only activate is once + if (auto_activated_plugins_.find(plugin.name_) != auto_activated_plugins_.end()) { - + plugin.user_requested_activation_ = false; } - } + plugin.active = true; // Mark plugin as active + + em_.update_entry(deactivated_entry); + } } - bool activate() + + void deactivate() { + // Bring all required or auto activated plugins to the active state + for (auto plugin : em_.get_entries()) + { + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_); - } - bool deactivate(std::vector plugins) - { + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be deactivated.") + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to deactivate non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!")); + Entry deactivated_entry = plugin; + deactivated_entry.active = false; + deactivated_entry.available = false; + deactivated_entry.user_requested_activation_ = false; + em_.update_entry(deactivated_entry); + } + + } } - bool cleanup(std::vector plugins) + + void cleanup() { + // Bring all required or auto activated plugins to the unconfigured state + for (auto plugin : em_.get_entries()) + { + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name_); + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be cleaned up.") + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to cleanup non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!")); + + Entry deactivated_entry = plugin; + deactivated_entry.active = false; + deactivated_entry.available = false; + deactivated_entry.user_requested_activation_ = false; + em_.update_entry(deactivated_entry); + } + + } } - bool shutdown(std::vector plugins) + + void shutdown() { + bool success = plugin_lifecycle_mgr_.shutdown(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms), false, em_.get_entries()).empty(); + + if (success) + { + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to shutdown"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to shutdown cleanly"); + return CallbackReturn::FAILURE; + } } void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginListRequest&, carma_planning_msgs::srv::PluginListResponse& res) @@ -177,7 +301,7 @@ namespace subsystem_controllers { boost::optional requested_plugin = em_.get_entry_by_name(req.plugin_name); - if(!requested_plugin) // If not a plugin then obviously not activated. Though really it would be better to have an indication of name failure in the message + if(!requested_plugin) // If not a known plugin then obviously not activated. Though really it would be better to have an indication of name failure in the message { res.newstate = false; return; @@ -187,7 +311,7 @@ namespace subsystem_controllers bool activated = (result_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - Entry updated_entry(requested_plugin->available_, activated, requested_plugin->name_, requested_plugin->type_, requested_plugin->capability_); + Entry updated_entry(requested_plugin->available_, activated, requested_plugin->name_, requested_plugin->type_, requested_plugin->capability_, true); // Mark as user activated em_.update_entry(updated_entry); res.newstate = activated; @@ -200,45 +324,97 @@ namespace subsystem_controllers if (!requested_plugin) // This is a new plugin so we need to add it { - Entry plugin(msg->available_, msg->activated_, msg->name_, msg->type_, msg->capability_); + Entry plugin(msg->available_, msg->activated_, msg->name_, msg->type_, msg->capability_, false); add_plugin(plugin); return; } - Entry plugin(msg->available, msg->activated_, msg->name, msg->type, msg->capability); + Entry plugin(msg->available, msg->activated_, msg->name, msg->type, msg->capability, requested_plugin->user_requested_activation_); em_.update_entry(plugin); } - bool PluginManager::get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) + bool PluginManager::matching_capability(const std::vector& base_capability_levels, const std::vector& compared_capability_levels) + { + for (size_t i=0; i < base_capability_levels.size() && i < compared_capability_levels.size(); i++) + { + if (compared_capability_levels[i].compare(req.capability) != 0) + return false; + } + + return true; + } + + void PluginManager::get_control_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) { + std::vector req_capability_levels; + boost::split(capability_levels, req.capability, boost::is_any_of("/")) + for(const auto& plugin : em_.get_entries()) { - if(plugin.type_ == carma_planning_msgs::msg::Plugin::TACTICAL && - (req.capability.size() == 0 || (plugin.capability_.compare(0, req.capability.size(), req.capability) == 0 && plugin.active_ && plugin.available_))) + + std::vector plugin_capability_levels; + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) + + if(plugin.type_ == carma_planning_msgs::msg::Plugin::CONTROL && + (req.capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { - res.plan_service.push_back(service_prefix_ + plugin.name_ + tactical_service_suffix_); + ROS_DEBUG_STREAM("discovered control plugin: " << plugin.name_); + res.plan_service.push_back(plugin.name_ + plugin.capability_); } + else + { + ROS_DEBUG_STREAM("not valid control plugin: " << plugin.name_); + } + } + } + + void PluginManager::get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) + { + std::vector req_capability_levels; + boost::split(capability_levels, req.capability, boost::is_any_of("/")) + + for(const auto& plugin : em_.get_entries()) + { + + std::vector plugin_capability_levels; + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) + + if(plugin.type_ == carma_planning_msgs::msg::Plugin::TACTICAL && + (req.capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) + { + ROS_DEBUG_STREAM("discovered tactical plugin: " << plugin.name_); + res.plan_service.push_back(plugin.name_ + plugin.capability_); + } + else + { + ROS_DEBUG_STREAM("not valid tactical plugin: " << plugin.name_); + } } - return true; } - bool PluginManager::get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) + void PluginManager::get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) { + std::vector req_capability_levels; + boost::split(capability_levels, req.capability, boost::is_any_of("/")) + for(const auto& plugin : em_.get_entries()) { + + std::vector plugin_capability_levels; + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) + if(plugin.type_ == carma_planning_msgs::msg::Plugin::STRATEGIC && - (req.capability.size() == 0 || (plugin.capability_.compare(0, req.capability.size(), req.capability) == 0 && plugin.active_ && plugin.available_))) + (req.capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { ROS_DEBUG_STREAM("discovered strategic plugin: " << plugin.name_); - res.plan_service.push_back(service_prefix_ + plugin.name_ + strategic_service_suffix_); + res.plan_service.push_back(plugin.name_ + plugin.capability_); } else { ROS_DEBUG_STREAM("not valid strategic plugin: " << plugin.name_); } } - return true; } } From bbbb0db73109875bb50743c861f3834e7e04a559 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Thu, 9 Jun 2022 21:05:04 +0000 Subject: [PATCH 018/165] fixing build errors --- .../guidance_controller.hpp | 20 +++-- .../guidance_controller_config.hpp | 5 +- .../guidance_controller/plugin_manager.h | 42 +++++++-- subsystem_controllers/package.xml | 1 + .../guidance_controller.cpp | 34 ++++---- .../guidance_controller/plugin_manager.cpp | 85 ++++++++++--------- 6 files changed, 113 insertions(+), 74 deletions(-) diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp index 404b023f12..aac2c29c93 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp @@ -20,14 +20,20 @@ #include -#include "carma_msgs/msg/system_alert.hpp" -#include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include #include "subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp" +#include "plugin_manager.h" +#include "guidance_controller_config.hpp" namespace subsystem_controllers { - using cr2 = carma_ros2_utils + namespace cr2 = carma_ros2_utils; class GuidanceControllerNode : public BaseSubsystemController { @@ -44,12 +50,16 @@ namespace subsystem_controllers */ explicit GuidanceControllerNode(const rclcpp::NodeOptions &options); - cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &); + cr2::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); cr2::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); cr2::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &); + cr2::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &); + + cr2::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &); + private: //! Plugin manager to handle all the plugin specific discovery and reporting std::shared_ptr plugin_manager_; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp index 220d540a26..324c2ee32d 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp @@ -42,9 +42,6 @@ namespace subsystem_controllers { output << "GuidanceControllerConfig { " << std::endl - << "required_plugins: " << c.required_plugins << std::endl - << "auto_activated_plugins: " << c.auto_activated_plugins << std::endl - << "required_plugins: [ " << std::endl; for (auto node : c.required_plugins) @@ -56,7 +53,7 @@ namespace subsystem_controllers output << node << " "; output << "] " << std::endl - << "}" << std::endl; + << "}" << std::endl; return output; } }; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index da85b178d8..221f881fba 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -17,10 +17,14 @@ */ #include -#include -#include #include +#include +#include #include +#include +#include +#include +#include #include "entry_manager.h" #include "entry.h" @@ -67,7 +71,31 @@ namespace subsystem_controllers * * \param msg A plugin status message */ - void update_plugin_status(const carma_planning_msgs::PluginConstPtr& msg); + void update_plugin_status(carma_planning_msgs::msg::Plugin::UniquePtr msg); + + /** + * \brief Returns the list of known plugins + * + * \param req The req details + * \param[out] res The response containing the list of known plugins + */ + void get_registered_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); + + /** + * \brief Get the list of currently active plugins + * + * \param req The req details + * \param[out] res The response containing the list of active plugins + */ + void get_active_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); + + /** + * \brief Activate the specified plugin + * + * \param req The req details containing the plugin to activate + * \param[out] res The response containing the success flag + */ + void activate_plugin(carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res); /** * \brief Get strategic plugins by capability @@ -75,7 +103,7 @@ namespace subsystem_controllers * \param req The req which identifies which capability is required * \param res The res which identifies the strategic plugins with the requested capability */ - void get_strategic_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); + void get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); /** * \brief Get tactical plugins by capability @@ -83,7 +111,7 @@ namespace subsystem_controllers * \param req The req which identifies which capability is required * \param res The res which identifies the tactical plugins with the requested capability */ - void get_tactical_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); + void get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); /** * \brief Get control plugins by capability @@ -91,7 +119,7 @@ namespace subsystem_controllers * \param req The req which identifies which capability is required * \param res The res which identifies the control plugins with the requested capability */ - void get_control_plugins_by_capability(carma_planning_msgs::GetPluginApiRequest& req, carma_planning_msgs::GetPluginApiResponse& res); + void get_control_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); protected: @@ -126,7 +154,7 @@ namespace subsystem_controllers * * \return True if base_capability_levels supports compared_capability_levels */ - bool matching_capability(const std::vector& base_capability_levels, const std::vector& compared_capability_levels) + bool matching_capability(const std::vector& base_capability_levels, const std::vector& compared_capability_levels); //! Set of required plugins a failure of which necessitates system shutdown std::unordered_set required_plugins_; diff --git a/subsystem_controllers/package.xml b/subsystem_controllers/package.xml index bdbb0e8490..d234e2f38a 100644 --- a/subsystem_controllers/package.xml +++ b/subsystem_controllers/package.xml @@ -29,6 +29,7 @@ carma_cmake_common carma_ros2_utils carma_msgs + carma_planning_msgs lifecycle_msgs rclcpp ros2_lifecycle_manager diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 8b3628560a..022b4b89b6 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -15,18 +15,20 @@ * the License. */ +#include #include "subsystem_controllers/guidance_controller/guidance_controller.hpp" namespace subsystem_controllers { GuidanceControllerNode::GuidanceControllerNode(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; - {} + } - cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &) { + cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { auto base_return = BaseSubsystemController::handle_on_configure(prev_state); if (base_return != cr2::CallbackReturn::SUCCESS) { @@ -34,18 +36,18 @@ namespace subsystem_controllers return base_return; } - config_ = Config(); + config_ = GuidanceControllerConfig(); auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes(); - std::string plugin_namespace = base_config_.subsystem_namespace + "/plugins/" + std::string plugin_namespace = base_config_.subsystem_namespace + "/plugins/"; std::vector guidance_plugin_nodes; // Extract the nodes under the plugin namespaces (ie. /guidance/plugins/) std::copy_if(base_managed_nodes.begin(), base_managed_nodes.end(), std::back_inserter(guidance_plugin_nodes), - [](const std::string& s) { return s.rfind(plugin_namespace, 0) == 0; }); + [plugin_namespace](const std::string& s) { return s.rfind(plugin_namespace, 0) == 0; }); std::vector non_plugin_guidance_nodes = get_non_intersecting_set(base_managed_nodes, guidance_plugin_nodes); @@ -58,7 +60,7 @@ namespace subsystem_controllers RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_); // The core need is that plugins need to be managed separately from guidance nodes - auto plugin_lifecycle_manager = std::make_shared( + auto plugin_lifecycle_manager = std::make_shared( get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface()); plugin_manager_ = std::make_shared(config_.required_plugins, config_.auto_activated_plugins, plugin_lifecycle_manager, [this](){ return get_current_state().id(); }); @@ -67,27 +69,27 @@ namespace subsystem_controllers "plugin_discovery", 50, std::bind(&PluginManager::update_plugin_status, plugin_manager_, std::placeholders::_1)); - get_registered_plugins_server_ = create_service( + get_registered_plugins_server_ = create_service( "plugins/get_registered_plugins", std::bind(&PluginManager::get_registered_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); - get_active_plugins_server_ = create_service( + get_active_plugins_server_ = create_service( "plugins/get_active_plugins", std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); - activate_plugin_server_ = create_service( + activate_plugin_server_ = create_service( "plugins/activate_plugin", // TODO check topic name std::bind(&PluginManager::activate_plugin, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); - get_strategic_plugin_by_capability_server_ = create_service( + get_strategic_plugin_by_capability_server_ = create_service( "plugins/get_strategic_plugin_by_capability", std::bind(&PluginManager::get_strategic_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); - get_tactical_plugin_by_capability_server_ = create_service( + get_tactical_plugin_by_capability_server_ = create_service( "plugins/get_tactical_plugin_by_capability", std::bind(&PluginManager::get_tactical_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); - get_control_plugin_by_capability_server_ = create_service( + get_control_plugin_by_capability_server_ = create_service( "plugins/get_control_plugin_by_capability", std::bind(&PluginManager::get_control_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); @@ -116,7 +118,7 @@ namespace subsystem_controllers return cr2::CallbackReturn::SUCCESS; } - cr2::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) + cr2::CallbackReturn GuidanceControllerNode::handle_on_activate(const rclcpp_lifecycle::State &prev_state) { auto base_return = BaseSubsystemController::handle_on_activate(prev_state); // This will activate all nodes in the namespace TODO what about the plugins? @@ -129,7 +131,7 @@ namespace subsystem_controllers } - cr2::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) + cr2::CallbackReturn GuidanceControllerNode::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) { auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); @@ -141,7 +143,7 @@ namespace subsystem_controllers plugin_manager_->deactivate(); // TODO callback return } - cr2::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) + cr2::CallbackReturn GuidanceControllerNode::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) { auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); @@ -153,7 +155,7 @@ namespace subsystem_controllers plugin_manager_->cleanup(); // TODO callback return } - cr2::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) + cr2::CallbackReturn GuidanceControllerNode::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) { auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 9c7d89625e..6b60758ef4 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -15,6 +15,7 @@ */ #include +#include #include "plugin_manager.h" namespace subsystem_controllers @@ -63,7 +64,7 @@ namespace subsystem_controllers deactivated_entry.active_ = false; // Move plugin to match this nodes state - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(parent_node_state, plugin.name); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(parent_node_state, plugin.name, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); if(result_state != parent_node_state) { @@ -75,8 +76,8 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to transition newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!")); + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to transition newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!"); deactivated_entry.available_ = false; @@ -90,7 +91,7 @@ namespace subsystem_controllers // If the node is active or inactive then, // when adding a plugin, it should be brought to the inactive state // We do not need transition it beyond inactive in this function as that will be managed by the plugin activation process via UI or parameters - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -101,8 +102,8 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to configure newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!")); + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!"); deactivated_entry.available_ = false; } @@ -112,13 +113,13 @@ namespace subsystem_controllers } - void configure() + void PluginManager::configure() { // Bring all known plugins to the inactive state for (auto plugin : em_.get_entries()) { - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -129,8 +130,8 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to configure newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!")); + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; deactivated_entry.active = false; @@ -143,7 +144,7 @@ namespace subsystem_controllers } - void activate() + void PluginManager::activate() { // Bring all required or auto activated plugins to the active state for (auto plugin : em_.get_entries()) @@ -152,7 +153,7 @@ namespace subsystem_controllers if (!plugin.user_requested_activation_) continue; - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, plugin.name_); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -163,8 +164,8 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to activate newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!")); + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to activate newly discovered non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; deactivated_entry.active = false; @@ -186,13 +187,13 @@ namespace subsystem_controllers } } - void deactivate() + void PluginManager::deactivate() { // Bring all required or auto activated plugins to the active state for (auto plugin : em_.get_entries()) { - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -203,8 +204,8 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to deactivate non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!")); + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to deactivate non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; deactivated_entry.active = false; @@ -216,13 +217,13 @@ namespace subsystem_controllers } } - void cleanup() + void PluginManager::cleanup() { // Bring all required or auto activated plugins to the unconfigured state for (auto plugin : em_.get_entries()) { - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name_); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { @@ -233,8 +234,8 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers", "Failed to cleanup non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!")); + RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to cleanup non-required plugin: " + << plugin.name << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; deactivated_entry.active = false; @@ -246,25 +247,25 @@ namespace subsystem_controllers } } - void shutdown() + void PluginManager::shutdown() { bool success = plugin_lifecycle_mgr_.shutdown(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms), false, em_.get_entries()).empty(); if (success) { - RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to shutdown"); + RCLCPP_INFO_STREAM(rclccp::get_logger("subsystem_controllers"), "Subsystem able to shutdown"); return CallbackReturn::SUCCESS; } else { - RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to shutdown cleanly"); + RCLCPP_INFO_STREAM(rclccp::get_logger("subsystem_controllers"), "Subsystem unable to shutdown cleanly"); return CallbackReturn::FAILURE; } } - void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginListRequest&, carma_planning_msgs::srv::PluginListResponse& res) + void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) { std::vector plugins = em_.get_entries(); // convert to plugin list @@ -279,7 +280,7 @@ namespace subsystem_controllers } } - void PluginManager::get_active_plugins(carma_planning_msgs::srv::PluginListRequest&, carma_planning_msgs::srv::PluginListResponse& res) + void PluginManager::get_active_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) { std::vector plugins = em_.get_entries(); // convert to plugin list @@ -297,7 +298,7 @@ namespace subsystem_controllers } } - void PluginManager::activate_plugin(cav_srvs::PluginActivationRequest& req, cav_srvs::PluginActivationResponse& res) + void PluginManager::activate_plugin(carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res) { boost::optional requested_plugin = em_.get_entry_by_name(req.plugin_name); @@ -307,7 +308,7 @@ namespace subsystem_controllers return; } - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, req.plugin_name); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, req.plugin_name, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); bool activated = (result_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); @@ -317,7 +318,7 @@ namespace subsystem_controllers res.newstate = activated; } - void PluginManager::update_plugin_status(const carma_planning_msgs::msg::PluginConstPtr& msg) + void PluginManager::update_plugin_status(carma_planning_msgs::msg::Plugin::UniquePtr msg) { ROS_DEBUG_STREAM("received status from: " << msg->name); boost::optional requested_plugin = em_.get_entry_by_name(msg->name); @@ -345,10 +346,10 @@ namespace subsystem_controllers return true; } - void PluginManager::get_control_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) + void PluginManager::get_control_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) { std::vector req_capability_levels; - boost::split(capability_levels, req.capability, boost::is_any_of("/")) + boost::split(capability_levels, req->capability, boost::is_any_of("/")) for(const auto& plugin : em_.get_entries()) { @@ -357,10 +358,10 @@ namespace subsystem_controllers boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) if(plugin.type_ == carma_planning_msgs::msg::Plugin::CONTROL && - (req.capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) + (req->capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { ROS_DEBUG_STREAM("discovered control plugin: " << plugin.name_); - res.plan_service.push_back(plugin.name_ + plugin.capability_); + res->plan_service.push_back(plugin.name_ + plugin.capability_); } else { @@ -369,10 +370,10 @@ namespace subsystem_controllers } } - void PluginManager::get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) + void PluginManager::get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) { std::vector req_capability_levels; - boost::split(capability_levels, req.capability, boost::is_any_of("/")) + boost::split(capability_levels, req->capability, boost::is_any_of("/")) for(const auto& plugin : em_.get_entries()) { @@ -381,10 +382,10 @@ namespace subsystem_controllers boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) if(plugin.type_ == carma_planning_msgs::msg::Plugin::TACTICAL && - (req.capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) + (req->capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { ROS_DEBUG_STREAM("discovered tactical plugin: " << plugin.name_); - res.plan_service.push_back(plugin.name_ + plugin.capability_); + res->plan_service.push_back(plugin.name_ + plugin.capability_); } else { @@ -393,10 +394,10 @@ namespace subsystem_controllers } } - void PluginManager::get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApiRequest& req, carma_planning_msgs::srv::GetPluginApiResponse& res) + void PluginManager::get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) { std::vector req_capability_levels; - boost::split(capability_levels, req.capability, boost::is_any_of("/")) + boost::split(capability_levels, req->capability, boost::is_any_of("/")) for(const auto& plugin : em_.get_entries()) { @@ -405,10 +406,10 @@ namespace subsystem_controllers boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) if(plugin.type_ == carma_planning_msgs::msg::Plugin::STRATEGIC && - (req.capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) + (req->capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { ROS_DEBUG_STREAM("discovered strategic plugin: " << plugin.name_); - res.plan_service.push_back(plugin.name_ + plugin.capability_); + res->plan_service.push_back(plugin.name_ + plugin.capability_); } else { From 1c6d375cb8c10bb6b28be058526231535ddbcb3b Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 10 Jun 2022 18:34:57 +0000 Subject: [PATCH 019/165] buildable guidance_controller --- subsystem_controllers/CMakeLists.txt | 6 +- .../guidance_controller/entry.h | 11 +- .../guidance_controller/entry_manager.h | 7 +- .../guidance_controller.hpp | 6 +- .../guidance_controller/plugin_manager.h | 34 ++- .../src/guidance_controller/entry_manager.cpp | 18 +- .../guidance_controller.cpp | 36 ++-- .../guidance_controller/plugin_manager.cpp | 199 +++++++++--------- 8 files changed, 185 insertions(+), 132 deletions(-) diff --git a/subsystem_controllers/CMakeLists.txt b/subsystem_controllers/CMakeLists.txt index b8aee952b4..fae77522b3 100644 --- a/subsystem_controllers/CMakeLists.txt +++ b/subsystem_controllers/CMakeLists.txt @@ -38,7 +38,11 @@ target_link_libraries(localization_controller localization_controller_core ${bas ament_auto_add_library(guidance_controller_core SHARED src/guidance_controller/guidance_controller.cpp) rclcpp_components_register_nodes(guidance_controller_core "subsystem_controllers::GuidanceControllerNode") target_link_libraries(guidance_controller_core ${base_lib}) -ament_auto_add_executable(guidance_controller src/guidance_controller/main.cpp) +ament_auto_add_executable(guidance_controller + src/guidance_controller/main.cpp + src/guidance_controller/entry_manager.cpp + src/guidance_controller/plugin_manager.cpp +) target_link_libraries(guidance_controller guidance_controller_core ${base_lib}) # Environment Subsystem diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h index 92d77edc4e..6d4bac5c93 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h @@ -26,22 +26,25 @@ namespace subsystem_controllers struct Entry { //! Availability flag of a plugin - bool available_; + bool available_ = false; //! Activation flag of a plugin - bool active_; + bool active_ = false; //! Fully specified node name of a plugin std::string name_; //! Type of the plugin from the message enum in carma_planning_msgs::Plugin - uint8_t type_; + uint8_t type_ = 0; //! The capability string of the plugin std::string capability_; //! Flag indicating if the user requested this plugin be activated - bool user_requested_activation_; + bool user_requested_activation_ = false; /** * \brief All fields constructor */ Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability, bool user_requested_activation) : available_(available), active_(active), name_(name), type_(type), capability_(capability), user_requested_activation_(user_requested_activation) {} + + + Entry() = default; }; } \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h index 1dc4264406..4dd5d870ca 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h @@ -33,7 +33,7 @@ namespace subsystem_controllers /*! * \brief Default constructor for EntryManager. */ - EntryManager(); + EntryManager() = default; /*! * \brief Add a new entry if the given name does not exist. @@ -48,6 +48,11 @@ namespace subsystem_controllers */ std::vector get_entries() const; + /*! + * \brief Get all entry names as a list + */ + std::vector get_entry_names() const; + /*! * \brief Get a entry using name as the key. */ diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp index aac2c29c93..227154e4c7 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp @@ -77,11 +77,11 @@ namespace subsystem_controllers cr2::ServicePtr activate_plugin_server_; - cr2::ServicePtr get_strategic_plugin_by_capability_server_; + cr2::ServicePtr get_strategic_plugins_by_capability_server_; - cr2::ServicePtr get_tactical_plugin_by_capability_server_; + cr2::ServicePtr get_tactical_plugins_by_capability_server_; - cr2::ServicePtr get_control_plugin_by_capability_server_; + cr2::ServicePtr get_control_plugins_by_capability_server_; }; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index 221f881fba..44fa5da004 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -25,6 +25,8 @@ #include #include #include +#include +#include #include "entry_manager.h" #include "entry.h" @@ -33,6 +35,7 @@ namespace subsystem_controllers { using GetParentNodeStateFunc = std::function; + using SrvHeader = const std::shared_ptr; /** * \brief The PluginManager serves as a component to manage CARMA Guidance Plugins via their ros2 lifecycle interfaces @@ -50,11 +53,14 @@ namespace subsystem_controllers * \param auto_activated_plugins The set of plugins which will be automatically activated at first system activation but not treated specially after that. * \param plugin_lifecycle_mgr A fully initialized lifecycle manager which will be used trigger plugin transitions * \param get_parent_state_func A callback which will allow this object to access the parent process lifecycle state + * \param service_timeout The timeout for plugin services to be available in nanoseconds + * \param call_timeout The timeout for calls to plugin services to fail in nanoseconds */ PluginManager(const std::vector& required_plugins, const std::vector& auto_activated_plugins, std::shared_ptr plugin_lifecycle_mgr, - GetParentNodeStateFunc get_parent_state_func); + GetParentNodeStateFunc get_parent_state_func, + std::chrono::nanoseconds service_timeout, std::chrono::nanoseconds call_timeout); /** * Below are the state transition methods which will cause this manager to trigger the corresponding @@ -79,47 +85,52 @@ namespace subsystem_controllers * \param req The req details * \param[out] res The response containing the list of known plugins */ - void get_registered_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); + void get_registered_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); /** * \brief Get the list of currently active plugins * + * \param header Middle ware header * \param req The req details * \param[out] res The response containing the list of active plugins */ - void get_active_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); + void get_active_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); /** * \brief Activate the specified plugin * + * \param header Middle ware header * \param req The req details containing the plugin to activate * \param[out] res The response containing the success flag */ - void activate_plugin(carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res); + void activate_plugin(SrvHeader, carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res); /** * \brief Get strategic plugins by capability * + * \param header Middle ware header * \param req The req which identifies which capability is required * \param res The res which identifies the strategic plugins with the requested capability */ - void get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); + void get_strategic_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); /** * \brief Get tactical plugins by capability * + * \param header Middle ware header * \param req The req which identifies which capability is required * \param res The res which identifies the tactical plugins with the requested capability */ - void get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); + void get_tactical_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); /** * \brief Get control plugins by capability * + * \param header Middle ware header * \param req The req which identifies which capability is required * \param res The res which identifies the control plugins with the requested capability */ - void get_control_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); + void get_control_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); protected: @@ -169,5 +180,14 @@ namespace subsystem_controllers //! Callback to retrieve the lifecycle state of the parent process GetParentNodeStateFunc get_parent_state_func_; + //! Entry manager to keep track of detected plugins + EntryManager em_; + + //! The timeout for services to be available + std::chrono::nanoseconds service_timeout_; + + //! The timeout for service calls to return + std::chrono::nanoseconds call_timeout_; + }; } \ No newline at end of file diff --git a/subsystem_controllers/src/guidance_controller/entry_manager.cpp b/subsystem_controllers/src/guidance_controller/entry_manager.cpp index a7af48736b..b1e396d66e 100644 --- a/subsystem_controllers/src/guidance_controller/entry_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/entry_manager.cpp @@ -14,14 +14,11 @@ * the License. */ -#include "entry_manager.h" -#include +#include "subsystem_controllers/guidance_controller/entry_manager.h" namespace subsystem_controllers { - EntryManager::EntryManager() {} - void EntryManager::update_entry(const Entry& entry) { entry_map_[entry.name_] = entry; @@ -40,6 +37,17 @@ namespace subsystem_controllers return entries; } + std::vector EntryManager::get_entry_names() const + { + std::vector names; + names.reserve(entry_map_.size()); + + for (const auto& e : entry_map_) + names.push_back(e.second.name_); + + return names; + } + void EntryManager::delete_entry(const std::string& name) { if (entry_map_.find(name) != entry_map_.end()) @@ -49,7 +57,7 @@ namespace subsystem_controllers boost::optional EntryManager::get_entry_by_name(const std::string& name) const { if (entry_map_.find(name) != entry_map_.end()) - return entry_map_[name]; + return entry_map_.at(name); // use boost::optional because requested entry might not exist diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 022b4b89b6..6e021e5181 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -18,10 +18,12 @@ #include #include "subsystem_controllers/guidance_controller/guidance_controller.hpp" +using std_msec = std::chrono::milliseconds; + namespace subsystem_controllers { GuidanceControllerNode::GuidanceControllerNode(const rclcpp::NodeOptions &options) - : BaseSubsystemController(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 @@ -63,7 +65,13 @@ namespace subsystem_controllers auto plugin_lifecycle_manager = std::make_shared( get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface()); - plugin_manager_ = std::make_shared(config_.required_plugins, config_.auto_activated_plugins, plugin_lifecycle_manager, [this](){ return get_current_state().id(); }); + plugin_manager_ = std::make_shared( + config_.required_plugins, + config_.auto_activated_plugins, + plugin_lifecycle_manager, + [this](){ return get_current_state().id(); }, + std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms) + ); plugin_discovery_sub_ = create_subscription( "plugin_discovery", 50, @@ -71,27 +79,27 @@ namespace subsystem_controllers get_registered_plugins_server_ = create_service( "plugins/get_registered_plugins", - std::bind(&PluginManager::get_registered_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + std::bind(&PluginManager::get_registered_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); get_active_plugins_server_ = create_service( "plugins/get_active_plugins", - std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); activate_plugin_server_ = create_service( "plugins/activate_plugin", // TODO check topic name - std::bind(&PluginManager::activate_plugin, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + std::bind(&PluginManager::activate_plugin, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - get_strategic_plugin_by_capability_server_ = create_service( - "plugins/get_strategic_plugin_by_capability", - std::bind(&PluginManager::get_strategic_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + get_strategic_plugins_by_capability_server_ = create_service( + "plugins/get_strategic_plugins_by_capability", + std::bind(&PluginManager::get_strategic_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - get_tactical_plugin_by_capability_server_ = create_service( - "plugins/get_tactical_plugin_by_capability", - std::bind(&PluginManager::get_tactical_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + get_tactical_plugins_by_capability_server_ = create_service( + "plugins/get_tactical_plugins_by_capability", + std::bind(&PluginManager::get_tactical_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - get_control_plugin_by_capability_server_ = create_service( - "plugins/get_control_plugin_by_capability", - std::bind(&PluginManager::get_control_plugin_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2)); + get_control_plugins_by_capability_server_ = create_service( + "plugins/get_control_plugins_by_capability", + std::bind(&PluginManager::get_control_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 6b60758ef4..2f3c5653cf 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -15,8 +15,12 @@ */ #include -#include -#include "plugin_manager.h" +#include +#include +#include +#include "subsystem_controllers/guidance_controller/plugin_manager.h" + +using std_msec = std::chrono::milliseconds; namespace subsystem_controllers { @@ -24,37 +28,42 @@ namespace subsystem_controllers PluginManager::PluginManager(const std::vector& required_plugins, const std::vector& auto_activated_plugins, std::shared_ptr plugin_lifecycle_mgr, - GetParentNodeStateFunc get_parent_state_func) // TODO should we pass pointer instead? - : required_plugins_(required_plugins.begin(), required_plugins_.end()), + GetParentNodeStateFunc get_parent_state_func, + std::chrono::nanoseconds service_timeout, std::chrono::nanoseconds call_timeout) + : required_plugins_(required_plugins.begin(), required_plugins.end()), auto_activated_plugins_(auto_activated_plugins.begin(), auto_activated_plugins.end()), - plugin_lifecycle_mgr_(plugin_lifecycle_mgr_), get_parent_state_func_(get_parent_state_func) + plugin_lifecycle_mgr_(plugin_lifecycle_mgr), get_parent_state_func_(get_parent_state_func), + service_timeout_(service_timeout), call_timeout_(call_timeout) { + if (!plugin_lifecycle_mgr) + throw std::invalid_argument("Input plugin_lifecycle_mgr to PluginManager constructor cannot be null"); + // For all required and auto activated plugins add unknown entries but with // user_requested_activation set to true. // This will be used later to determine how to transition the plugin specified by that entry for (const auto& p : required_plugins_) { - Entry e(false, false, p.name, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); em_.update_entry(e); - plugin_lifecycle_mgr_->add_managed_node(p.name); + plugin_lifecycle_mgr_->add_managed_node(p); } for (const auto& p : auto_activated_plugins_) { - Entry e(false, false, p.name, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); em_.update_entry(e); - plugin_lifecycle_mgr_->add_managed_node(p.name); + plugin_lifecycle_mgr_->add_managed_node(p); } } void PluginManager::add_plugin(const Entry& plugin) { - plugin_lifecycle_mgr_->add_managed_node(plugin.name); + plugin_lifecycle_mgr_->add_managed_node(plugin.name_); em_.update_entry(plugin); Entry deactivated_entry = plugin; - auto parent_node_state = get_parent_state_func(); + auto parent_node_state = get_parent_state_func_(); // If this node is not in the active or inactive states then move the plugin to unconfigured if (parent_node_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE @@ -64,20 +73,20 @@ namespace subsystem_controllers deactivated_entry.active_ = false; // Move plugin to match this nodes state - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(parent_node_state, plugin.name, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(parent_node_state, plugin.name_, service_timeout_, call_timeout_); if(result_state != parent_node_state) { // If this plugin was required then trigger exception - if (required_plugins_.find(plugin.name) != required_plugins_.end()) + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) { - throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to controller node state."); + throw std::runtime_error("Newly discovered required plugin " + plugin.name_ + " could not be brought to controller node state."); } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to transition newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to transition newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); deactivated_entry.available_ = false; @@ -91,24 +100,24 @@ namespace subsystem_controllers // If the node is active or inactive then, // when adding a plugin, it should be brought to the inactive state // We do not need transition it beyond inactive in this function as that will be managed by the plugin activation process via UI or parameters - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, service_timeout_, call_timeout_); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { // If this plugin was required then trigger exception - if (required_plugins_.find(plugin.name) != required_plugins_.end()) + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) { - throw std::runtime_error("Newly discovered required plugin " + plugin.name + " could not be brought to inactive state."); + throw std::runtime_error("Newly discovered required plugin " + plugin.name_ + " could not be brought to inactive state."); } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); deactivated_entry.available_ = false; } - deactivated_entry.active = false; + deactivated_entry.active_ = false; em_.update_entry(deactivated_entry); } @@ -119,7 +128,7 @@ namespace subsystem_controllers // Bring all known plugins to the inactive state for (auto plugin : em_.get_entries()) { - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, service_timeout_, call_timeout_); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -130,12 +139,12 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; - deactivated_entry.active = false; - deactivated_entry.available = false; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; deactivated_entry.user_requested_activation_ = false; // TODO is there an edge case where the user hits this first???? em_.update_entry(deactivated_entry); } @@ -153,7 +162,7 @@ namespace subsystem_controllers if (!plugin.user_requested_activation_) continue; - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, plugin.name_, service_timeout_, call_timeout_); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { @@ -164,12 +173,12 @@ namespace subsystem_controllers } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to activate newly discovered non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to activate newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; - deactivated_entry.active = false; - deactivated_entry.available = false; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; deactivated_entry.user_requested_activation_ = false; em_.update_entry(deactivated_entry); } @@ -180,9 +189,9 @@ namespace subsystem_controllers plugin.user_requested_activation_ = false; } - plugin.active = true; // Mark plugin as active + plugin.active_ = true; // Mark plugin as active - em_.update_entry(deactivated_entry); + em_.update_entry(plugin); } } @@ -193,23 +202,23 @@ namespace subsystem_controllers for (auto plugin : em_.get_entries()) { - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, service_timeout_, call_timeout_); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { // If this plugin was required then trigger exception if (required_plugins_.find(plugin.name_) != required_plugins_.end()) { - throw std::runtime_error("Required plugin " + plugin.name_ + " could not be deactivated.") + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be deactivated."); } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to deactivate non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to deactivate non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; - deactivated_entry.active = false; - deactivated_entry.available = false; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; deactivated_entry.user_requested_activation_ = false; em_.update_entry(deactivated_entry); } @@ -223,23 +232,23 @@ namespace subsystem_controllers for (auto plugin : em_.get_entries()) { - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name_, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name_, service_timeout_, call_timeout_); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { // If this plugin was required then trigger exception if (required_plugins_.find(plugin.name_) != required_plugins_.end()) { - throw std::runtime_error("Required plugin " + plugin.name_ + " could not be cleaned up.") + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be cleaned up."); } // If this plugin was not required log an error and mark it is unavailable and deactivated - RCLCPP_ERROR_STREAM(rclccp::get_logger("subsystem_controllers"), "Failed to cleanup non-required plugin: " - << plugin.name << " Marking as deactivated and unavailable!"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to cleanup non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); Entry deactivated_entry = plugin; - deactivated_entry.active = false; - deactivated_entry.available = false; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; deactivated_entry.user_requested_activation_ = false; em_.update_entry(deactivated_entry); } @@ -249,88 +258,84 @@ namespace subsystem_controllers void PluginManager::shutdown() { - bool success = plugin_lifecycle_mgr_.shutdown(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms), false, em_.get_entries()).empty(); + bool success = plugin_lifecycle_mgr_->shutdown(service_timeout_, call_timeout_, false, em_.get_entry_names()).empty(); if (success) { - RCLCPP_INFO_STREAM(rclccp::get_logger("subsystem_controllers"), "Subsystem able to shutdown"); - return CallbackReturn::SUCCESS; + RCLCPP_INFO_STREAM(rclcpp::get_logger("subsystem_controllers"), "Subsystem able to shutdown"); } else { - RCLCPP_INFO_STREAM(rclccp::get_logger("subsystem_controllers"), "Subsystem unable to shutdown cleanly"); - return CallbackReturn::FAILURE; + RCLCPP_INFO_STREAM(rclcpp::get_logger("subsystem_controllers"), "Subsystem unable to shutdown cleanly"); } } - void PluginManager::get_registered_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) + void PluginManager::get_registered_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) { - std::vector plugins = em_.get_entries(); // convert to plugin list - for(const auto& plugin : plugin_map_) + for(const auto& plugin : em_.get_entries()) { - carma_planning_msgs::msg::Plugin plugin; - plugin.activated = i->active_; - plugin.available = i->available_; - plugin.name = i->name_; - plugin.type = i->type_; - res.plugins.push_back(plugin); + carma_planning_msgs::msg::Plugin msg; + msg.activated = plugin.active_; + msg.available = plugin.available_; + msg.name = plugin.name_; + msg.type = plugin.type_; + res->plugins.push_back(msg); } } - void PluginManager::get_active_plugins(carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) + void PluginManager::get_active_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) { - std::vector plugins = em_.get_entries(); // convert to plugin list - for(auto i = plugins.begin(); i < plugins.end(); ++i) + for(const auto& plugin : em_.get_entries()) { - if(i->active_) + if(plugin.active_) { - carma_planning_msgs::msg::Plugin plugin; - plugin.activated = i->active_; - plugin.available = i->available_; - plugin.name = i->name_; - plugin.type = i->type_; - res.plugins.push_back(plugin); + carma_planning_msgs::msg::Plugin msg; + msg.activated = plugin.active_; + msg.available = plugin.available_; + msg.name = plugin.name_; + msg.type = plugin.type_; + res->plugins.push_back(msg); } } } - void PluginManager::activate_plugin(carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res) + void PluginManager::activate_plugin(SrvHeader, carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res) { - boost::optional requested_plugin = em_.get_entry_by_name(req.plugin_name); + boost::optional requested_plugin = em_.get_entry_by_name(req->plugin_name); if(!requested_plugin) // If not a known plugin then obviously not activated. Though really it would be better to have an indication of name failure in the message { - res.newstate = false; + res->newstate = false; return; } - auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, req.plugin_name, std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)); + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, requested_plugin->name_, service_timeout_, call_timeout_); bool activated = (result_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); Entry updated_entry(requested_plugin->available_, activated, requested_plugin->name_, requested_plugin->type_, requested_plugin->capability_, true); // Mark as user activated em_.update_entry(updated_entry); - res.newstate = activated; + res->newstate = activated; } void PluginManager::update_plugin_status(carma_planning_msgs::msg::Plugin::UniquePtr msg) { - ROS_DEBUG_STREAM("received status from: " << msg->name); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "received status from: " << msg->name); boost::optional requested_plugin = em_.get_entry_by_name(msg->name); if (!requested_plugin) // This is a new plugin so we need to add it { - Entry plugin(msg->available_, msg->activated_, msg->name_, msg->type_, msg->capability_, false); + Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, false); add_plugin(plugin); return; } - Entry plugin(msg->available, msg->activated_, msg->name, msg->type, msg->capability, requested_plugin->user_requested_activation_); + Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, requested_plugin->user_requested_activation_); em_.update_entry(plugin); } @@ -339,81 +344,81 @@ namespace subsystem_controllers { for (size_t i=0; i < base_capability_levels.size() && i < compared_capability_levels.size(); i++) { - if (compared_capability_levels[i].compare(req.capability) != 0) + if (compared_capability_levels[i].compare(base_capability_levels[i]) != 0) return false; } return true; } - void PluginManager::get_control_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) + void PluginManager::get_control_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) { std::vector req_capability_levels; - boost::split(capability_levels, req->capability, boost::is_any_of("/")) + boost::split(req_capability_levels, req->capability, boost::is_any_of("/")); for(const auto& plugin : em_.get_entries()) { std::vector plugin_capability_levels; - boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")); if(plugin.type_ == carma_planning_msgs::msg::Plugin::CONTROL && - (req->capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) + (req->capability.size() == 0 || (matching_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { - ROS_DEBUG_STREAM("discovered control plugin: " << plugin.name_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "discovered control plugin: " << plugin.name_); res->plan_service.push_back(plugin.name_ + plugin.capability_); } else { - ROS_DEBUG_STREAM("not valid control plugin: " << plugin.name_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "not valid control plugin: " << plugin.name_); } } } - void PluginManager::get_tactical_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) + void PluginManager::get_tactical_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) { std::vector req_capability_levels; - boost::split(capability_levels, req->capability, boost::is_any_of("/")) + boost::split(req_capability_levels, req->capability, boost::is_any_of("/")); for(const auto& plugin : em_.get_entries()) { std::vector plugin_capability_levels; - boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")); if(plugin.type_ == carma_planning_msgs::msg::Plugin::TACTICAL && - (req->capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) + (req->capability.size() == 0 || (matching_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { - ROS_DEBUG_STREAM("discovered tactical plugin: " << plugin.name_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "discovered tactical plugin: " << plugin.name_); res->plan_service.push_back(plugin.name_ + plugin.capability_); } else { - ROS_DEBUG_STREAM("not valid tactical plugin: " << plugin.name_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "not valid tactical plugin: " << plugin.name_); } } } - void PluginManager::get_strategic_plugins_by_capability(carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) + void PluginManager::get_strategic_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) { std::vector req_capability_levels; - boost::split(capability_levels, req->capability, boost::is_any_of("/")) + boost::split(req_capability_levels, req->capability, boost::is_any_of("/")); for(const auto& plugin : em_.get_entries()) { std::vector plugin_capability_levels; - boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")) + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")); if(plugin.type_ == carma_planning_msgs::msg::Plugin::STRATEGIC && - (req->capability.size() == 0 || (match_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) + (req->capability.size() == 0 || (matching_capability(plugin_capability_levels, req_capability_levels) == 0 && plugin.active_ && plugin.available_))) { - ROS_DEBUG_STREAM("discovered strategic plugin: " << plugin.name_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "discovered strategic plugin: " << plugin.name_); res->plan_service.push_back(plugin.name_ + plugin.capability_); } else { - ROS_DEBUG_STREAM("not valid strategic plugin: " << plugin.name_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "not valid strategic plugin: " << plugin.name_); } } } From 23204fe904dea6c6dbc478cd8407199ea2044d16 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 10 Jun 2022 18:48:53 +0000 Subject: [PATCH 020/165] handling todo --- .../guidance_controller/plugin_manager.h | 13 ++- .../guidance_controller.cpp | 91 +++++++++++++++++-- .../guidance_controller/plugin_manager.cpp | 46 ++++++---- 3 files changed, 116 insertions(+), 34 deletions(-) diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index 44fa5da004..a1ab469441 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -65,12 +65,15 @@ namespace subsystem_controllers /** * Below are the state transition methods which will cause this manager to trigger the corresponding * state transitions in the managed plugins. + * + * \throw std::runtime_error If a required node could not transition successfully + * \return True if all components transitioned successfully */ - void configure(); - void activate(); - void deactivate(); - void cleanup(); - void shutdown(); + bool configure(); + bool activate(); + bool deactivate(); + bool cleanup(); + bool shutdown(); /** * \brief Update the status of a certain plugin diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 6e021e5181..a49a0bbc1e 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -107,7 +107,12 @@ namespace subsystem_controllers bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); // Configure our plugins - plugin_manager_->configure(); // TODO callback returns for this + try { + plugin_manager_->configure(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + if (success) @@ -123,7 +128,6 @@ namespace subsystem_controllers return CallbackReturn::FAILURE; } - return cr2::CallbackReturn::SUCCESS; } cr2::CallbackReturn GuidanceControllerNode::handle_on_activate(const rclcpp_lifecycle::State &prev_state) @@ -135,7 +139,25 @@ namespace subsystem_controllers return base_return; } - plugin_manager_->activate(); // TODO callback return + bool success = true; + try { + plugin_manager_->activate(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to activate"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to activate"); + return CallbackReturn::FAILURE; + } } @@ -148,31 +170,80 @@ namespace subsystem_controllers return base_return; } - plugin_manager_->deactivate(); // TODO callback return + bool success = true; + try { + plugin_manager_->deactivate(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to deactivate"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to deactivate"); + return CallbackReturn::FAILURE; + } } cr2::CallbackReturn GuidanceControllerNode::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) { - auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); + auto base_return = BaseSubsystemController::handle_on_cleanup(prev_state); if (base_return != cr2::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(get_logger(), "Guidance Controller could not deactivate"); + RCLCPP_ERROR(get_logger(), "Guidance Controller could not cleanup"); return base_return; } - plugin_manager_->cleanup(); // TODO callback return + bool success = true; + try { + plugin_manager_->cleanup(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to cleanup"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to cleanup"); + return CallbackReturn::FAILURE; + } } cr2::CallbackReturn GuidanceControllerNode::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) { - auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); + auto base_return = BaseSubsystemController::handle_on_shutdown(prev_state); if (base_return != cr2::CallbackReturn::SUCCESS) { - RCLCPP_ERROR(get_logger(), "Guidance Controller could not deactivate"); + RCLCPP_ERROR(get_logger(), "Guidance Controller could not shutdown"); return base_return; } - plugin_manager_->shutdown(); // TODO callback return + bool success = plugin_manager_->shutdown(); + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to shutdown cleanly"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to shutdown cleanly"); + return CallbackReturn::FAILURE; + } } diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 2f3c5653cf..f7c549fedb 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -122,9 +122,9 @@ namespace subsystem_controllers } - void PluginManager::configure() + bool PluginManager::configure() { - + bool full_success = true; // Bring all known plugins to the inactive state for (auto plugin : em_.get_entries()) { @@ -145,16 +145,21 @@ namespace subsystem_controllers Entry deactivated_entry = plugin; deactivated_entry.active_ = false; deactivated_entry.available_ = false; - deactivated_entry.user_requested_activation_ = false; // TODO is there an edge case where the user hits this first???? + deactivated_entry.user_requested_activation_ = false; em_.update_entry(deactivated_entry); + + full_success = false; } } + return full_success; + } - void PluginManager::activate() + bool PluginManager::activate() { + bool full_success = true; // Bring all required or auto activated plugins to the active state for (auto plugin : em_.get_entries()) { @@ -181,6 +186,8 @@ namespace subsystem_controllers deactivated_entry.available_ = false; deactivated_entry.user_requested_activation_ = false; em_.update_entry(deactivated_entry); + + full_success = false; } // If this was an auto activated plugin and not required then we only activate is once @@ -194,10 +201,13 @@ namespace subsystem_controllers em_.update_entry(plugin); } + + return full_success; } - void PluginManager::deactivate() + bool PluginManager::deactivate() { + bool full_success = true; // Bring all required or auto activated plugins to the active state for (auto plugin : em_.get_entries()) { @@ -221,13 +231,18 @@ namespace subsystem_controllers deactivated_entry.available_ = false; deactivated_entry.user_requested_activation_ = false; em_.update_entry(deactivated_entry); + + full_success = false; } } + + return full_success; } - void PluginManager::cleanup() + bool PluginManager::cleanup() { + bool full_success = true; // Bring all required or auto activated plugins to the unconfigured state for (auto plugin : em_.get_entries()) { @@ -251,25 +266,18 @@ namespace subsystem_controllers deactivated_entry.available_ = false; deactivated_entry.user_requested_activation_ = false; em_.update_entry(deactivated_entry); + + full_success = false; } } + + return full_success; } - void PluginManager::shutdown() + bool PluginManager::shutdown() { - bool success = plugin_lifecycle_mgr_->shutdown(service_timeout_, call_timeout_, false, em_.get_entry_names()).empty(); - - if (success) - { - - RCLCPP_INFO_STREAM(rclcpp::get_logger("subsystem_controllers"), "Subsystem able to shutdown"); - } - else - { - - RCLCPP_INFO_STREAM(rclcpp::get_logger("subsystem_controllers"), "Subsystem unable to shutdown cleanly"); - } + return plugin_lifecycle_mgr_->shutdown(service_timeout_, call_timeout_, false, em_.get_entry_names()).empty(); } void PluginManager::get_registered_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) From bfe81595312dbc3aa56f407d94e4375d73341a84 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 10 Jun 2022 19:06:22 +0000 Subject: [PATCH 021/165] Add parameters to config --- .../config/guidance_controller_config.yaml | 20 +++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 1937bdd577..2962437c98 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -9,7 +9,7 @@ call_timeout_ms : 1000 # String: The namespace for nodes in this subsystem. All nodes under this namespace will have their lifecycle's managed by this controller - subsystem_namespace: /guidance # TODO we need to decouple this from the plugins + subsystem_namespace: /guidance # Required subsystem controller nodes for the overall system to be functional and will have their lifecycle's managed required_subsystem_nodes: @@ -30,4 +30,20 @@ unmanaged_required_nodes: [''] # TODO add the controller driver once it is integrated with ROS2 # 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: true \ No newline at end of file + full_subsystem_required: false + + # List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. + # Required plugins will be automatically activated at startup + # Required plugins cannot be deactivated by the user + required_plugins: + - /guidance/plugins/route_following_plugin + - /guidance/plugins/pure_pursuit_wrapper + - /guidance/plugins/inlane_cruising_plugin + # TODO determine if there are any more + + # List of guidance plugins which are not required but the user wishes to have automatically activated + # so that the user doesn't need to manually activate them via the UI on each launch (though they still can) + # this list should have zero intersection with the required_plugins + auto_activated_plugins: + - /guidance/plugins/lic_strategic_plugin + # TODO add all our current plugins \ No newline at end of file From 336184cc278f97e609e50604546125a45c02f1b9 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Wed, 22 Jun 2022 13:35:42 -0700 Subject: [PATCH 022/165] fix message spec in lci --- .../lci_strategic_plugin.h | 6 ----- .../src/lci_strategic_plugin.cpp | 25 +++---------------- .../test/test_strategic_plugin.cpp | 5 +--- 3 files changed, 5 insertions(+), 31 deletions(-) diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h index ba87152938..db2af91976 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h @@ -195,12 +195,6 @@ class LCIStrategicPlugin unsigned long long scheduled_stop_time_ = 0; // scheduled enter time unsigned long long scheduled_enter_time_ = 0; - // scheduled depart time - unsigned long long scheduled_depart_time_ = 0; - // scheduled latest depart position - uint32_t scheduled_departure_position_ = std::numeric_limits::max(); - // flag to show if the vehicle is allowed in intersection - bool is_allowed_int_ = false; //BSM std::string bsm_id_ = "default_bsm_id"; diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index 4c1e3e8fe8..b8ae01e8b0 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -892,7 +892,7 @@ void LCIStrategicPlugin::BSMCb(const cav_msgs::BSMConstPtr& msg) void LCIStrategicPlugin::parseStrategyParams(const std::string& strategy_params) { - // sample strategy_params: "st:1634067044,et:1634067059, dt:1634067062.3256602,dp:2,,access: 0" + // sample strategy_params: "st:1634067044,et:1634067059" std::string params = strategy_params; std::vector inputsParams; boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); @@ -907,23 +907,6 @@ void LCIStrategicPlugin::parseStrategyParams(const std::string& strategy_params) scheduled_enter_time_ = std::stoull(et_parsed[1]); ROS_DEBUG_STREAM("scheduled_enter_time_: " << scheduled_enter_time_); - std::vector dt_parsed; - boost::algorithm::split(dt_parsed, inputsParams[2], boost::is_any_of(":")); - scheduled_depart_time_ = std::stoull(dt_parsed[1]); - ROS_DEBUG_STREAM("scheduled_depart_time_: " << scheduled_depart_time_); - - - std::vector dp_parsed; - boost::algorithm::split(dp_parsed, inputsParams[3], boost::is_any_of(":")); - scheduled_departure_position_ = std::stoi(dp_parsed[1]); - ROS_DEBUG_STREAM("scheduled_departure_position_: " << scheduled_departure_position_); - - std::vector access_parsed; - boost::algorithm::split(access_parsed, inputsParams[4], boost::is_any_of(":")); - int access = std::stoi(access_parsed[1]); - is_allowed_int_ = (access == 1); - ROS_DEBUG_STREAM("is_allowed_int_: " << is_allowed_int_); - } cav_msgs::MobilityOperation LCIStrategicPlugin::generateMobilityOperation() @@ -942,9 +925,9 @@ cav_msgs::MobilityOperation LCIStrategicPlugin::generateMobilityOperation() if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left"; - mo_.strategy_params = "access: " + std::to_string(is_allowed_int_) + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + + mo_.strategy_params = "max_accel: " + std::to_string(vehicle_acceleration_limit_) + ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) + - ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: " + std::to_string(scheduled_departure_position_) + + ", min_gap: " + std::to_string(config_.min_gap) + ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_); @@ -1012,7 +995,7 @@ bool LCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav return true; } - bool is_empty_schedule_msg = (scheduled_depart_time_ == 0 && scheduled_stop_time_ == 0 && scheduled_enter_time_ == 0); + bool is_empty_schedule_msg = (scheduled_stop_time_ == 0 && scheduled_enter_time_ == 0); if (is_empty_schedule_msg) { resp.new_plan.maneuvers = {}; diff --git a/lci_strategic_plugin/test/test_strategic_plugin.cpp b/lci_strategic_plugin/test/test_strategic_plugin.cpp index 1ebeb7f480..57998cac94 100755 --- a/lci_strategic_plugin/test/test_strategic_plugin.cpp +++ b/lci_strategic_plugin/test/test_strategic_plugin.cpp @@ -400,7 +400,7 @@ TEST(LCIStrategicPluginTest, moboperationcbtest) TEST(LCIStrategicPluginTest, parseStrategyParamstest) { cav_msgs::MobilityOperation msg; - msg.strategy_params = "st:16000,et:32000,dt:48000,dp:1,access:0"; + msg.strategy_params = "st:16000,et:32000"; std::shared_ptr wm = std::make_shared(); LCIStrategicPluginConfig config; @@ -411,9 +411,6 @@ TEST(LCIStrategicPluginTest, parseStrategyParamstest) EXPECT_EQ(16000, lcip.scheduled_stop_time_); EXPECT_EQ(32000, lcip.scheduled_enter_time_); - EXPECT_EQ(48000, lcip.scheduled_depart_time_); - EXPECT_EQ(1, lcip.scheduled_departure_position_); - EXPECT_EQ(false, lcip.is_allowed_int_); cav_msgs::MobilityOperation outgoing_msg = lcip.generateMobilityOperation(); EXPECT_EQ(outgoing_msg.strategy, "Carma/light_controlled_intersection"); From 2926efcbcd1c0b0580a9074a59f07e6629b13db0 Mon Sep 17 00:00:00 2001 From: Kyle Rush Date: Thu, 23 Jun 2022 17:05:16 -0400 Subject: [PATCH 023/165] Add checks to ensure objects are in front of vehicle (#1714) * Add checks to ensure objecst are in front of vehicle Added checks to yield plugin to ignore objects at or behind the vehicle in downtrack distance. * Remove unnecessary comment RE: std::abs Co-authored-by: Michael McConnell --- yield_plugin/src/yield_plugin.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index edfb596ed4..cad4215ccf 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -447,21 +447,21 @@ namespace yield_plugin ROS_DEBUG_STREAM("object_down_track"); ROS_DEBUG_STREAM(object_down_track); - ROS_DEBUG_STREAM("vehicle_downtrack - object_down_track"); - ROS_DEBUG_STREAM(vehicle_downtrack - object_down_track); + double dist_to_object = object_down_track - vehicle_downtrack; + ROS_DEBUG_STREAM("object_down_track - vehicle_downtrack"); + ROS_DEBUG_STREAM(dist_to_object); ROS_DEBUG_STREAM("i.object.velocity.twist.linear.x"); ROS_DEBUG_STREAM(i.object.velocity.twist.linear.x); if(current_velocity.linear.x > 0.0) { - - // std::abs might not be needed cause vehicles in the behind of vehicle to cause problem - - ROS_DEBUG_STREAM("std::abs(vehicle_downtrack - object_down_track)/current_velocity.linear.x"); - - ROS_DEBUG_STREAM(object_down_track - vehicle_downtrack/current_velocity.linear.x); + ROS_DEBUG_STREAM("(object_down_downtrack - vehicle_downtrack)/current_velocity.linear.x"); + ROS_DEBUG_STREAM(dist_to_object/current_velocity.linear.x); - if((object_down_track - vehicle_downtrack)/current_velocity.linear.x < config_.collision_horizon) { + // 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); } } From 0a74d6f95622588de88ece72d66ed05b66deb81b Mon Sep 17 00:00:00 2001 From: Aswath <32398753+aswath1@users.noreply.github.com> Date: Sat, 25 Jun 2022 01:45:05 +0530 Subject: [PATCH 024/165] Feature/ros2 pose to tf (#1827) Description Port the pose_to_tf node from ROS1 to ROS2 Related Issue #1834 --- pose_to_tf/CMakeLists.txt | 105 ++++++++---------- .../pose_to_tf/{PoseToTF2.h => PoseToTF2.hpp} | 32 +++--- .../include/pose_to_tf/PoseToTF2Config.h | 26 ----- .../include/pose_to_tf/PoseToTF2Config.hpp | 41 +++++++ .../include/pose_to_tf/PoseToTF2Node.hpp | 62 +++++++++++ pose_to_tf/launch/pose_to_tf.launch | 18 --- pose_to_tf/launch/pose_to_tf_launch.py | 62 +++++++++++ pose_to_tf/package.xml | 30 +++-- pose_to_tf/src/PoseToTF2.cpp | 49 ++++---- pose_to_tf/src/PoseToTF2Node.cpp | 60 ++++++++++ pose_to_tf/src/main.cpp | 37 ++---- pose_to_tf/test/TestMain.cpp | 26 ----- pose_to_tf/test/TestPoseToTF2.cpp | 86 +++++++++----- 13 files changed, 404 insertions(+), 230 deletions(-) rename pose_to_tf/include/pose_to_tf/{PoseToTF2.h => PoseToTF2.hpp} (63%) delete mode 100644 pose_to_tf/include/pose_to_tf/PoseToTF2Config.h create mode 100644 pose_to_tf/include/pose_to_tf/PoseToTF2Config.hpp create mode 100644 pose_to_tf/include/pose_to_tf/PoseToTF2Node.hpp delete mode 100644 pose_to_tf/launch/pose_to_tf.launch create mode 100644 pose_to_tf/launch/pose_to_tf_launch.py create mode 100644 pose_to_tf/src/PoseToTF2Node.cpp delete mode 100644 pose_to_tf/test/TestMain.cpp diff --git a/pose_to_tf/CMakeLists.txt b/pose_to_tf/CMakeLists.txt index 9f0b3edb39..216dc0f83e 100644 --- a/pose_to_tf/CMakeLists.txt +++ b/pose_to_tf/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2018-2020 LEIDOS. +# 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 @@ -14,81 +14,68 @@ # the License. # -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(pose_to_tf) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -set(ROS_DEPS - geometry_msgs - roscpp - tf2 - tf2_ros - tf2_geometry_msgs - carma_utils -) -find_package(catkin REQUIRED COMPONENTS - ${ROS_DEPS} -) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - CATKIN_DEPENDS ${ROS_DEPS} - LIBRARIES ${PROJECT_NAME} -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -########### -## Build ## -########### +# Name build targets +set(worker_lib PoseToTF2_worker_lib) +set(node_exec PoseToTF2Node_node_exec) +set(node_lib PoseToTF2Node_node) +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} ) -add_library( ${PROJECT_NAME} - src/PoseToTF2.cpp) +# Build +ament_auto_add_library(${worker_lib} + src/PoseToTF2.cpp +) +ament_auto_add_library(${node_lib} SHARED + src/PoseToTF2Node.cpp +) +ament_auto_add_executable(${node_exec} + src/main.cpp +) -add_dependencies( ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +# Register component +rclcpp_components_register_nodes(${node_lib} "pose_to_tf::PoseToTfNode") +# 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} +) -add_executable( ${PROJECT_NAME}_node - src/main.cpp) +target_link_libraries(${node_exec} + ${node_lib} +) -add_dependencies( ${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES}) +# Testing +if(BUILD_TESTING) -############# -## Install ## -############# + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) + ament_add_gtest(test_pose_to_tf test/TestPoseToTF2.cpp) -## Install C++ -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ament_target_dependencies(test_pose_to_tf + ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + target_link_libraries(test_pose_to_tf ${node_lib}) -############# -## Testing ## -############# -catkin_add_gmock(${PROJECT_NAME}-test test/TestMain.cpp - test/TestPoseToTF2.cpp -) -target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES}) +endif() + + # Install +ament_auto_package( + INSTALL_TO_SHARE launch config +) \ No newline at end of file diff --git a/pose_to_tf/include/pose_to_tf/PoseToTF2.h b/pose_to_tf/include/pose_to_tf/PoseToTF2.hpp similarity index 63% rename from pose_to_tf/include/pose_to_tf/PoseToTF2.h rename to pose_to_tf/include/pose_to_tf/PoseToTF2.hpp index c4986d211c..5e12a234e6 100644 --- a/pose_to_tf/include/pose_to_tf/PoseToTF2.h +++ b/pose_to_tf/include/pose_to_tf/PoseToTF2.hpp @@ -15,14 +15,15 @@ * the License. */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include +#include namespace pose_to_tf { @@ -32,47 +33,48 @@ namespace pose_to_tf class PoseToTF2 { public: - using TransformPublisher = std::function; + using TransformPublisher = std::function; /** * \brief Constructor * * \param transform_pub A callback to trigger transform broadcast */ - PoseToTF2(PoseToTF2Config config, TransformPublisher transform_pub); + PoseToTF2(PoseToTF2Config config, TransformPublisher transform_pub,std::shared_ptr node); /** * \brief Callback for new pose stamped messages * * \param msg The pose message to forward */ - void poseStampedCallback(const geometry_msgs::PoseStampedConstPtr& msg); + void poseStampedCallback(geometry_msgs::msg::PoseStamped::UniquePtr msg); /** * \brief Callback for new pose stamped messages * * \param msg The pose message to forward */ - void poseWithCovarianceStampedCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); + void poseWithCovarianceStampedCallback(geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr msg); /** * \brief Callback for new pose stamped messages * * \param msg The pose message to forward */ - void poseCallback(const geometry_msgs::PoseConstPtr& msg); + void poseCallback(geometry_msgs::msg::Pose::UniquePtr msg); /** * \brief Callback for new pose with covariance messages * * \param msg The pose message to forward */ - void poseWithCovarianceCallback(const geometry_msgs::PoseWithCovarianceConstPtr& msg); + void poseWithCovarianceCallback(geometry_msgs::msg::PoseWithCovariance::UniquePtr msg); private: + std::shared_ptr node_; + PoseToTF2Config config_; TransformPublisher transform_pub_; }; - -} // namespace pose_to_tf +}//namespace pose_to_tf diff --git a/pose_to_tf/include/pose_to_tf/PoseToTF2Config.h b/pose_to_tf/include/pose_to_tf/PoseToTF2Config.h deleted file mode 100644 index c43170c349..0000000000 --- a/pose_to_tf/include/pose_to_tf/PoseToTF2Config.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - -#include -namespace pose_to_tf -{ -struct PoseToTF2Config -{ - std::string child_frame = "base_link"; - std::string default_parent_frame = "map"; -}; -} // namespace pose_to_tf \ No newline at end of file diff --git a/pose_to_tf/include/pose_to_tf/PoseToTF2Config.hpp b/pose_to_tf/include/pose_to_tf/PoseToTF2Config.hpp new file mode 100644 index 0000000000..b2b85749a6 --- /dev/null +++ b/pose_to_tf/include/pose_to_tf/PoseToTF2Config.hpp @@ -0,0 +1,41 @@ +#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 + +namespace pose_to_tf +{ + /** + * \brief Stuct containing the frame configuration values for pose_to_tf + */ + + struct PoseToTF2Config + { + std::string child_frame = "base_link"; + std::string default_parent_frame = "map"; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const PoseToTF2Config &c) + { + output << "pose_to_tf::PoseToTF2Config { " << std::endl + << "child_frame: " << c.child_frame << std::endl + << "default_parent_frame: " << c.default_parent_frame << std::endl + << "}" << std::endl; + return output; + } + }; +} // namespace pose_to_tf \ No newline at end of file diff --git a/pose_to_tf/include/pose_to_tf/PoseToTF2Node.hpp b/pose_to_tf/include/pose_to_tf/PoseToTF2Node.hpp new file mode 100644 index 0000000000..0ef1d6d39c --- /dev/null +++ b/pose_to_tf/include/pose_to_tf/PoseToTF2Node.hpp @@ -0,0 +1,62 @@ +#pragma once +/* + * Copyright (C) 2020-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 "pose_to_tf/PoseToTF2.hpp" +#include "pose_to_tf/PoseToTF2Config.hpp" +#include + +namespace pose_to_tf +{ + + /** + * \class PoseToTfNode + * \brief The class is responsible for processing pose to tf conversion. + */ + + class PoseToTfNode : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Subscribers + carma_ros2_utils::SubPtr pose_sub; + carma_ros2_utils::SubPtr pose_stamped_sub; + carma_ros2_utils::SubPtr pose_with_cov_sub; + carma_ros2_utils::SubPtr pose_with_cov_stamped_sub; + + //PoseToTF2 class object + std::shared_ptr pose_to_tf_worker_; + + // Node configuration + PoseToTF2Config config_; + + public: + + /** + * \brief PoseToTfNode constructor + */ + explicit PoseToTfNode(const rclcpp::NodeOptions &); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + + }; + +} // pose_to_tf diff --git a/pose_to_tf/launch/pose_to_tf.launch b/pose_to_tf/launch/pose_to_tf.launch deleted file mode 100644 index 7f668cbaab..0000000000 --- a/pose_to_tf/launch/pose_to_tf.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - diff --git a/pose_to_tf/launch/pose_to_tf_launch.py b/pose_to_tf/launch/pose_to_tf_launch.py new file mode 100644 index 0000000000..3d67fc9890 --- /dev/null +++ b/pose_to_tf/launch/pose_to_tf_launch.py @@ -0,0 +1,62 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA pose_to_tf. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='pose_to_tf_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='pose_to_tf', + plugin='pose_to_tf::PoseToTfNode', + name='pose_to_tf_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/pose_to_tf/package.xml b/pose_to_tf/package.xml index 318122d692..9506da2fae 100644 --- a/pose_to_tf/package.xml +++ b/pose_to_tf/package.xml @@ -1,7 +1,7 @@ - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index e9014ccec9..11b858f5ab 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -41,7 +41,7 @@ def generate_launch_description(): strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate') tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate') control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate') - + vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file') declare_vehicle_config_param_file_arg = DeclareLaunchArgument( name = 'vehicle_config_param_file', @@ -61,6 +61,9 @@ def generate_launch_description(): route_param_file = os.path.join( get_package_share_directory('route'), 'config/parameters.yaml') + plan_delegator_param_file = os.path.join( + get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml') + env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') subsystem_controller_param_file = LaunchConfiguration('subsystem_controller_param_file') @@ -78,7 +81,29 @@ def generate_launch_description(): executable='carma_component_container_mt', namespace=GetCurrentNamespace(), composable_node_descriptions=[ - + ComposableNode( + package='plan_delegator', + plugin='plan_delegator::PlanDelegator', + name='plan_delegator', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('plan_delegator', env_log_levels) } + ], + remappings = [ + ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ("vehicle_status", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle_status" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("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" ] ), + ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ) + ], + parameters=[ + plan_delegator_param_file, + vehicle_config_param_file + ] + ), ComposableNode( package='mobilitypath_visualizer', plugin='mobilitypath_visualizer::MobilityPathVisualizer', @@ -135,7 +160,7 @@ def generate_launch_description(): route_param_file, vehicle_config_param_file ] - ), + ) ] ) diff --git a/plan_delegator/CMakeLists.txt b/plan_delegator/CMakeLists.txt index cd4b420f2d..eaacfb8d84 100644 --- a/plan_delegator/CMakeLists.txt +++ b/plan_delegator/CMakeLists.txt @@ -1,4 +1,4 @@ -# Copyright (C) 2018-2021 LEIDOS. +# 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 @@ -12,112 +12,60 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(plan_delegator) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - cav_msgs - cav_srvs - roscpp - std_msgs - carma_utils - carma_wm - tf - tf2 - tf2_geometry_msgs -) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES plan_delegator - CATKIN_DEPENDS cav_msgs cav_srvs roscpp std_msgs carma_utils carma_wm tf tf2 tf2_geometry_msgs -# DEPENDS system_lib -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -########### -## Build ## -########### +# Name build targets +set(node_exec plan_delegator_exec) +set(node_lib plan_delegator_node) -## Specify additional locations of header files -## Your package locations should be listed before other locations +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} ) -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node - src/plan_delegator_node.cpp) +# Build +ament_auto_add_library(${node_lib} SHARED + src/plan_delegator.cpp +) -add_library(${PROJECT_NAME}_lib src/plan_delegator.cpp) +ament_auto_add_executable(${node_exec} + src/plan_delegator_node.cpp +) -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(${PROJECT_NAME}_lib ${catkin_EXPORTED_TARGETS}) +# Register component +rclcpp_components_register_nodes(${node_lib} "plan_delegator::PlanDelegator") -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_lib - ${catkin_LIBRARIES} -) -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME}_lib +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -############# -## Install ## -############# +#Testing +if(BUILD_TESTING) -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_lib - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) + ament_add_gtest(test_plan_delegator test/test_plan_delegator.cpp) -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + ament_target_dependencies(test_plan_delegator ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -############# -## Testing ## -############# -#catkin_add_gmock(${PROJECT_NAME}-test test/test_plan_delegator.cpp) + target_link_libraries(test_plan_delegator ${node_lib}) -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_plan_delegator test/plan_delegator.test test/test_plan_delegator.cpp) - target_link_libraries(test_plan_delegator ${PROJECT_NAME}_lib ${catkin_LIBRARIES}) endif() +# Install +ament_auto_package( + INSTALL_TO_SHARE launch config +) \ No newline at end of file diff --git a/plan_delegator/config/plan_delegator_params.yaml b/plan_delegator/config/plan_delegator_params.yaml index 3c16668bc7..00aac2041d 100644 --- a/plan_delegator/config/plan_delegator_params.yaml +++ b/plan_delegator/config/plan_delegator_params.yaml @@ -20,4 +20,4 @@ trajectory_duration_threshold: 6.0 # Minimum crawl speed # Double: The minimum speed the vehicle will say it is moving in order to support accelerations # Units: m/s -min_speed: 2.2352 +min_speed: 2.2352 \ No newline at end of file diff --git a/plan_delegator/include/plan_delegator.hpp b/plan_delegator/include/plan_delegator.hpp index dff5a58236..64a8cdfbf0 100644 --- a/plan_delegator/include/plan_delegator.hpp +++ b/plan_delegator/include/plan_delegator.hpp @@ -1,5 +1,6 @@ +#pragma once /* - * Copyright (C) 2019-2022 LEIDOS. + * 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 @@ -14,24 +15,22 @@ * the License. */ -#ifndef PLAN_DELEGATOR_INCLUDE_PLAN_DELEGATOR_HPP_ -#define PLAN_DELEGATOR_INCLUDE_PLAN_DELEGATOR_HPP_ - #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include +#include +#include +#include +#include // TODO Replace this Macro if possible /** @@ -41,74 +40,94 @@ * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field */ #define GET_MANEUVER_PROPERTY(mvr, property)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\ throw new std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id")))))))) #define SET_MANEUVER_PROPERTY(mvr, property, value)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property = (value) :\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property = (value) :\ throw std::invalid_argument("ADJUST_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))))) namespace plan_delegator { - class PlanDelegator + /** + * \brief Config struct + */ + struct Config + { + // ROS params + std::string planning_topic_prefix = "/plugins/"; + std::string planning_topic_suffix = "/plan_trajectory"; + double trajectory_planning_rate = 10.0; + double max_trajectory_duration = 6.0; + double min_crawl_speed = 2.2352; // Min crawl speed in m/s + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "PlanDelegator::Config { " << std::endl + << "planning_topic_prefix: " << c.planning_topic_prefix << std::endl + << "planning_topic_suffix: " << c.planning_topic_suffix << std::endl + << "trajectory_planning_rate: " << c.trajectory_planning_rate << std::endl + << "max_trajectory_duration: " << c.max_trajectory_duration << std::endl + << "min_crawl_speed: " << c.min_crawl_speed << std::endl + << "}" << std::endl; + return output; + } + }; + + class PlanDelegator : public carma_ros2_utils::CarmaLifecycleNode { public: // constants definition static const constexpr double MILLISECOND_TO_SECOND = 0.001; - PlanDelegator() = default; - /** - * \brief Initialize the plan delegator + * \brief PlanDelegator constructor */ - void init(); - - /** - * \brief Run the spin loop of plan delegator - */ - void run(); + explicit PlanDelegator(const rclcpp::NodeOptions &); /** * \brief Callback function of maneuver plan subscriber */ - void maneuverPlanCallback(const cav_msgs::ManeuverPlanConstPtr& plan); + void maneuverPlanCallback(carma_planning_msgs::msg::ManeuverPlan::UniquePtr plan); /** * \brief Callback function of guidance state subscriber */ - void guidanceStateCallback(const cav_msgs::GuidanceStateConstPtr& plan); + void guidanceStateCallback(carma_planning_msgs::msg::GuidanceState::UniquePtr plan); /** * \brief Get PlanTrajectory service client by plugin name and * create new PlanTrajectory service client if specified name does not exist * \return a ServiceClient object which corresponse to the target planner */ - ros::ServiceClient& getPlannerClientByName(const std::string& planner_name); + carma_ros2_utils::ClientPtr getPlannerClientByName(const std::string& planner_name); /** * \brief Example if a maneuver end time has passed current system time * \return if input maneuver is expires + * NOTE: current_time is assumed to be same clock type as this node */ - bool isManeuverExpired(const cav_msgs::Maneuver& maneuver, ros::Time current_time = ros::Time::now()) const; + bool isManeuverExpired(const carma_planning_msgs::msg::Maneuver& maneuver, rclcpp::Time current_time) const; /** * \brief Generate new PlanTrajecory service request based on current planning progress - * \return a PlanTrajectory object which is ready to be used in the following service call + * \return a PlanTrajectoryRequest which is ready to be used in the following service call */ - cav_srvs::PlanTrajectory composePlanTrajectoryRequest(const cav_msgs::TrajectoryPlan& latest_trajectory_plan, const uint16_t& current_maneuver_index) const; + std::shared_ptr composePlanTrajectoryRequest(const carma_planning_msgs::msg::TrajectoryPlan& latest_trajectory_plan, const uint16_t& current_maneuver_index) const; /** * \brief Lookup transfrom from front bumper to base link @@ -121,41 +140,37 @@ namespace plan_delegator * are shifted based on the distance between the base_link frame and the vehicle_front frame. * \param maneuver The maneuver to be updated. */ - void updateManeuverParameters(cav_msgs::Maneuver& maneuver); + void updateManeuverParameters(carma_planning_msgs::msg::Maneuver& maneuver); - protected: - - // ROS params - std::string planning_topic_prefix_ = ""; - std::string planning_topic_suffix_ = ""; - double trajectory_planning_rate_ = 10.0; - double max_trajectory_duration_ = 6.0; - double min_crawl_speed_ = 2.2352; // Min crawl speed in m/s + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + protected: + // Node configuration + Config config_; + // map to store service clients - std::unordered_map trajectory_planners_; + std::unordered_map> trajectory_planners_; // local storage of incoming messages - cav_msgs::ManeuverPlan latest_maneuver_plan_; - geometry_msgs::PoseStamped latest_pose_; - geometry_msgs::TwistStamped latest_twist_; + carma_planning_msgs::msg::ManeuverPlan latest_maneuver_plan_; + geometry_msgs::msg::PoseStamped latest_pose_; + geometry_msgs::msg::TwistStamped latest_twist_; // wm listener pointer and pointer to the actual wm object - std::shared_ptr wml_; + carma_wm::WMListener wml_; carma_wm::WorldModelConstPtr wm_; private: - - // nodehandle and private nodehandle - ros::NodeHandle nh_; - ros::NodeHandle pnh_; - // ROS subscribers and publishers - ros::Publisher traj_pub_; - ros::Subscriber plan_sub_; - ros::Subscriber pose_sub_; - ros::Subscriber twist_sub_; - ros::Subscriber guidance_state_sub_; - ros::Timer traj_timer_; + carma_ros2_utils::PubPtr traj_pub_; + carma_ros2_utils::SubPtr plan_sub_; + carma_ros2_utils::SubPtr pose_sub_; + carma_ros2_utils::SubPtr twist_sub_; + carma_ros2_utils::SubPtr guidance_state_sub_; + rclcpp::TimerBase::SharedPtr traj_timer_; bool guidance_engaged = false; @@ -168,32 +183,31 @@ namespace plan_delegator /** * \brief Callback function for triggering trajectory planning */ - void onTrajPlanTick(const ros::TimerEvent& te); + void onTrajPlanTick(); /** * \brief Example if a maneuver plan contains at least one maneuver * \return if input maneuver plan is valid */ - bool isManeuverPlanValid(const cav_msgs::ManeuverPlanConstPtr& maneuver_plan) const noexcept; + bool isManeuverPlanValid(const carma_planning_msgs::msg::ManeuverPlan& maneuver_plan) const noexcept; /** * \brief Example if a trajectory plan contains at least two trajectory points * \return if input trajectory plan is valid */ - bool isTrajectoryValid(const cav_msgs::TrajectoryPlan& trajectory_plan) const noexcept; + bool isTrajectoryValid(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const noexcept; /** * \brief Example if a trajectory plan is longer than configured time thresheld * \return if input trajectory plan is long enough */ - bool isTrajectoryLongEnough(const cav_msgs::TrajectoryPlan& plan) const noexcept; + bool isTrajectoryLongEnough(const carma_planning_msgs::msg::TrajectoryPlan& plan) const noexcept; /** * \brief Plan trajectory based on latest maneuver plan via ROS service call to plugins * \return a TrajectoryPlan object which contains PlanTrajectory response from plugins */ - cav_msgs::TrajectoryPlan planTrajectory(); + carma_planning_msgs::msg::TrajectoryPlan planTrajectory(); }; -} -#endif // PLAN_DELEGATOR_INCLUDE_PLAN_DELEGATOR_HPP_ \ No newline at end of file +} \ No newline at end of file diff --git a/plan_delegator/launch/plan_delegator.launch b/plan_delegator/launch/plan_delegator.launch deleted file mode 100644 index 893136556a..0000000000 --- a/plan_delegator/launch/plan_delegator.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - diff --git a/plan_delegator/launch/plan_delegator.launch.py b/plan_delegator/launch/plan_delegator.launch.py new file mode 100644 index 0000000000..132354a342 --- /dev/null +++ b/plan_delegator/launch/plan_delegator.launch.py @@ -0,0 +1,67 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the Plan Delegator Node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + plan_delegator_param_file = os.path.join( + get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='plan_delegator_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='plan_delegator', + plugin='plan_delegator::PlanDelegator', + name='plan_delegator', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ plan_delegator_param_file ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/plan_delegator/package.xml b/plan_delegator/package.xml index eecb7de4e3..c3f368d7e6 100644 --- a/plan_delegator/package.xml +++ b/plan_delegator/package.xml @@ -1,7 +1,7 @@ plan_delegator - 3.3.0 + 4.0.0 The plan_delegator package @@ -15,21 +15,28 @@ Apache 2.0 - catkin + ament_cmake carma_cmake_common - cav_msgs - cav_srvs - roscpp + ament_auto_cmake + + carma_planning_msgs + rclcpp std_msgs - carma_utils - carma_wm + carma_ros2_utils + carma_wm_ros2 tf tf2 tf2_geometry_msgs - - - + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + diff --git a/plan_delegator/src/plan_delegator.cpp b/plan_delegator/src/plan_delegator.cpp index dfb1897759..1094dadda1 100644 --- a/plan_delegator/src/plan_delegator.cpp +++ b/plan_delegator/src/plan_delegator.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2022 LEIDOS. + * 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 @@ -15,11 +15,12 @@ */ #include -#include +#include #include "plan_delegator.hpp" namespace plan_delegator { + namespace std_ph = std::placeholders; namespace { @@ -28,23 +29,23 @@ namespace plan_delegator * maneuver parameter cannot be set with SET_MANEUVER_PROPERTY calls since it is not included in * LANE_FOLLOW maneuvers. */ - void setManeuverStartingLaneletId(cav_msgs::Maneuver& mvr, lanelet::Id start_id) { - ROS_DEBUG_STREAM("Updating maneuver starting_lane_id to " << start_id); + void setManeuverStartingLaneletId(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id start_id) { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Updating maneuver starting_lane_id to " << start_id); switch(mvr.type) { - case cav_msgs::Maneuver::LANE_CHANGE: + case carma_planning_msgs::msg::Maneuver::LANE_CHANGE: mvr.lane_change_maneuver.starting_lane_id = std::to_string(start_id); break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: mvr.intersection_transit_straight_maneuver.starting_lane_id = std::to_string(start_id); break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: mvr.intersection_transit_left_turn_maneuver.starting_lane_id = std::to_string(start_id); break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: mvr.intersection_transit_right_turn_maneuver.starting_lane_id = std::to_string(start_id); break; - case cav_msgs::Maneuver::STOP_AND_WAIT: + case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT: mvr.stop_and_wait_maneuver.starting_lane_id = std::to_string(start_id); break; default: @@ -57,23 +58,23 @@ namespace plan_delegator * maneuver parameter cannot be set with SET_MANEUVER_PROPERTY calls since it is not included in * LANE_FOLLOW maneuvers. */ - void setManeuverEndingLaneletId(cav_msgs::Maneuver& mvr, lanelet::Id end_id) { - ROS_DEBUG_STREAM("Updating maneuver ending_lane_id to " << end_id); + void setManeuverEndingLaneletId(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id end_id) { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Updating maneuver ending_lane_id to " << end_id); switch(mvr.type) { - case cav_msgs::Maneuver::LANE_CHANGE: + case carma_planning_msgs::msg::Maneuver::LANE_CHANGE: mvr.lane_change_maneuver.ending_lane_id = std::to_string(end_id); break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: mvr.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(end_id); break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: mvr.intersection_transit_left_turn_maneuver.ending_lane_id = std::to_string(end_id); break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: mvr.intersection_transit_right_turn_maneuver.ending_lane_id = std::to_string(end_id); break; - case cav_msgs::Maneuver::STOP_AND_WAIT: + case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT: mvr.stop_and_wait_maneuver.ending_lane_id = std::to_string(end_id); break; default: @@ -86,17 +87,17 @@ namespace plan_delegator * maneuver parameters cannot be obtained with GET_MANEUVER_PROPERTY calls since they are not included in * LANE_FOLLOW maneuvers. */ - std::string getManeuverStartingLaneletId(cav_msgs::Maneuver mvr) { + std::string getManeuverStartingLaneletId(carma_planning_msgs::msg::Maneuver mvr) { switch(mvr.type) { - case cav_msgs::Maneuver::LANE_CHANGE: + case carma_planning_msgs::msg::Maneuver::LANE_CHANGE: return mvr.lane_change_maneuver.starting_lane_id; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: return mvr.intersection_transit_straight_maneuver.starting_lane_id; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: return mvr.intersection_transit_left_turn_maneuver.starting_lane_id; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: return mvr.intersection_transit_right_turn_maneuver.starting_lane_id; - case cav_msgs::Maneuver::STOP_AND_WAIT: + case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT: return mvr.stop_and_wait_maneuver.starting_lane_id; default: throw std::invalid_argument("Maneuver type does not have starting and ending lane ids"); @@ -108,76 +109,87 @@ namespace plan_delegator * maneuver parameters cannot be obtained with GET_MANEUVER_PROPERTY calls since they are not included in * LANE_FOLLOW maneuvers. */ - std::string getManeuverEndingLaneletId(cav_msgs::Maneuver mvr) { + std::string getManeuverEndingLaneletId(carma_planning_msgs::msg::Maneuver mvr) { switch(mvr.type) { - case cav_msgs::Maneuver::LANE_CHANGE: + case carma_planning_msgs::msg::Maneuver::LANE_CHANGE: return mvr.lane_change_maneuver.ending_lane_id; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: return mvr.intersection_transit_straight_maneuver.ending_lane_id; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: return mvr.intersection_transit_left_turn_maneuver.ending_lane_id; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: + case carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: return mvr.intersection_transit_right_turn_maneuver.ending_lane_id; - case cav_msgs::Maneuver::STOP_AND_WAIT: + case carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT: return mvr.stop_and_wait_maneuver.ending_lane_id; default: throw std::invalid_argument("Maneuver type does not have starting and ending lane ids"); } } - } - - void PlanDelegator::init() + } //namespace + + + PlanDelegator::PlanDelegator(const rclcpp::NodeOptions &options) : carma_ros2_utils::CarmaLifecycleNode(options), + tf2_buffer_(this->get_clock()), + wml_(this->get_node_base_interface(), this->get_node_logging_interface(), + this->get_node_topics_interface(), this->get_node_parameters_interface()) { - nh_ = ros::CARMANodeHandle(); - pnh_ = ros::CARMANodeHandle("~"); + // Create initial config + config_ = Config(); + + config_.planning_topic_prefix = declare_parameter("planning_topic_prefix", config_.planning_topic_prefix); + config_.planning_topic_suffix = declare_parameter("planning_topic_suffix", config_.planning_topic_suffix); + config_.trajectory_planning_rate = declare_parameter("trajectory_planning_rate", config_.trajectory_planning_rate); + config_.max_trajectory_duration = declare_parameter("trajectory_duration_threshold", config_.max_trajectory_duration); + config_.min_crawl_speed = declare_parameter("min_speed", config_.min_crawl_speed); + } - // Initialize world model - wml_.reset(new carma_wm::WMListener()); + carma_ros2_utils::CallbackReturn PlanDelegator::handle_on_configure(const rclcpp_lifecycle::State &) + { + // Reset config + config_ = Config(); - + get_parameter("planning_topic_prefix", config_.planning_topic_prefix); + get_parameter("planning_topic_suffix", config_.planning_topic_suffix); + get_parameter("trajectory_planning_rate", config_.trajectory_planning_rate); + get_parameter("trajectory_duration_threshold", config_.max_trajectory_duration); + get_parameter("min_speed", config_.min_crawl_speed); - pnh_.param("planning_topic_prefix", planning_topic_prefix_, "/plugins/"); - pnh_.param("planning_topic_suffix", planning_topic_suffix_, "/plan_trajectory"); - pnh_.param("trajectory_planning_rate", trajectory_planning_rate_, 10.0); - pnh_.param("trajectory_duration_threshold", max_trajectory_duration_, 6.0); - pnh_.param("min_speed", min_crawl_speed_, min_crawl_speed_); + RCLCPP_INFO_STREAM(get_logger(),"Done loading parameters: " << config_); - traj_pub_ = nh_.advertise("plan_trajectory", 5); - plan_sub_ = nh_.subscribe("final_maneuver_plan", 5, &PlanDelegator::maneuverPlanCallback, this); - twist_sub_ = nh_.subscribe("current_velocity", 5, - [this](const geometry_msgs::TwistStampedConstPtr& twist) {this->latest_twist_ = *twist;}); - pose_sub_ = nh_.subscribe("current_pose", 5, - [this](const geometry_msgs::PoseStampedConstPtr& pose) {this->latest_pose_ = *pose;}); - guidance_state_sub_ = nh_.subscribe("guidance_state", 5, &PlanDelegator::guidanceStateCallback, this); + traj_pub_ = create_publisher("plan_trajectory", 5); + plan_sub_ = create_subscription("final_maneuver_plan", 5, std::bind(&PlanDelegator::maneuverPlanCallback, this, std_ph::_1)); + twist_sub_ = create_subscription("current_velocity", 5, + [this](geometry_msgs::msg::TwistStamped::UniquePtr twist) {this->latest_twist_ = *twist;}); + pose_sub_ = create_subscription("current_pose", 5, + [this](geometry_msgs::msg::PoseStamped::UniquePtr pose) {this->latest_pose_ = *pose;}); + guidance_state_sub_ = create_subscription("guidance_state", 5, std::bind(&PlanDelegator::guidanceStateCallback, this, std_ph::_1)); lookupFrontBumperTransform(); - wm_ = wml_->getWorldModel(); - - traj_timer_ = pnh_.createTimer( - ros::Duration(ros::Rate(trajectory_planning_rate_)), - &PlanDelegator::onTrajPlanTick, - this); + wm_ = wml_.getWorldModel(); + return CallbackReturn::SUCCESS; } - - void PlanDelegator::run() + + carma_ros2_utils::CallbackReturn PlanDelegator::handle_on_activate(const rclcpp_lifecycle::State &) { - ros::CARMANodeHandle::spin(); + traj_timer_ = create_timer(get_clock(), + std::chrono::milliseconds((int)(config_.trajectory_planning_rate * 1000)), + std::bind(&PlanDelegator::onTrajPlanTick, this)); + return CallbackReturn::SUCCESS; } - - void PlanDelegator::guidanceStateCallback(const cav_msgs::GuidanceStateConstPtr& msg) + + void PlanDelegator::guidanceStateCallback(carma_planning_msgs::msg::GuidanceState::UniquePtr msg) { - guidance_engaged = (msg->state == cav_msgs::GuidanceState::ENGAGED); + guidance_engaged = (msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED); } - void PlanDelegator::maneuverPlanCallback(const cav_msgs::ManeuverPlanConstPtr& plan) + void PlanDelegator::maneuverPlanCallback(carma_planning_msgs::msg::ManeuverPlan::UniquePtr plan) { - ROS_INFO_STREAM("Received request to delegate plan ID " << plan->maneuver_plan_id); + RCLCPP_INFO_STREAM(get_logger(),"Received request to delegate plan ID " << plan->maneuver_plan_id); // do basic check to see if the input is valid - if (isManeuverPlanValid(plan)) + if (isManeuverPlanValid(*plan)) { latest_maneuver_plan_ = *plan; - - ROS_DEBUG_STREAM("Received plan with " << latest_maneuver_plan_.maneuvers.size() << " maneuvers"); + RCLCPP_DEBUG_STREAM(get_logger(),"Received plan with " << latest_maneuver_plan_.maneuvers.size() << " maneuvers"); // Update the parameters associated with each maneuver for (auto& maneuver : latest_maneuver_plan_.maneuvers) { @@ -185,11 +197,11 @@ namespace plan_delegator } } else { - ROS_WARN_STREAM("Received empty plan, no maneuvers found in plan ID " << plan->maneuver_plan_id); + RCLCPP_WARN_STREAM(get_logger(),"Received empty plan, no maneuvers found in plan ID " << plan->maneuver_plan_id); } } - ros::ServiceClient& PlanDelegator::getPlannerClientByName(const std::string& planner_name) + carma_ros2_utils::ClientPtr PlanDelegator::getPlannerClientByName(const std::string& planner_name) { if(planner_name.size() == 0) { @@ -197,89 +209,95 @@ namespace plan_delegator } if(trajectory_planners_.find(planner_name) == trajectory_planners_.end()) { - ROS_INFO_STREAM("Discovered new trajectory planner: " << planner_name); + RCLCPP_INFO_STREAM(get_logger(),"Discovered new trajectory planner: " << planner_name); trajectory_planners_.emplace( - planner_name, nh_.serviceClient(planning_topic_prefix_ + planner_name + planning_topic_suffix_)); + planner_name, create_client(config_.planning_topic_prefix + planner_name + config_.planning_topic_suffix)); } return trajectory_planners_[planner_name]; } - bool PlanDelegator::isManeuverPlanValid(const cav_msgs::ManeuverPlanConstPtr& maneuver_plan) const noexcept + bool PlanDelegator::isManeuverPlanValid(const carma_planning_msgs::msg::ManeuverPlan& maneuver_plan) const noexcept { // currently it only checks if maneuver list is empty - return !maneuver_plan->maneuvers.empty(); + return !maneuver_plan.maneuvers.empty(); } - bool PlanDelegator::isTrajectoryValid(const cav_msgs::TrajectoryPlan& trajectory_plan) const noexcept + bool PlanDelegator::isTrajectoryValid(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const noexcept { // currently it only checks if trajectory contains less than 2 points return !(trajectory_plan.trajectory_points.size() < 2); } - bool PlanDelegator::isManeuverExpired(const cav_msgs::Maneuver& maneuver, ros::Time current_time) const + bool PlanDelegator::isManeuverExpired(const carma_planning_msgs::msg::Maneuver& maneuver, rclcpp::Time current_time) const { - ROS_DEBUG_STREAM("maneuver start time:" << std::to_string(GET_MANEUVER_PROPERTY(maneuver, start_time).toSec())); - ROS_DEBUG_STREAM("maneuver end time:" << std::to_string(GET_MANEUVER_PROPERTY(maneuver, end_time).toSec())); - ROS_DEBUG_STREAM("current time:" << std::to_string(ros::Time::now().toSec())); - bool isexpired = GET_MANEUVER_PROPERTY(maneuver, end_time) <= current_time; - ROS_DEBUG_STREAM("isexpired:" << isexpired); + RCLCPP_DEBUG_STREAM(get_logger(), "maneuver start time:" << std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, start_time)).seconds())); + RCLCPP_DEBUG_STREAM(get_logger(), "maneuver end time:" << std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time)).seconds())); + RCLCPP_DEBUG_STREAM(get_logger(), "current time:" << std::to_string(now().seconds())); + bool isexpired = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time), get_clock()->get_clock_type()) <= current_time; // TODO maneuver expiration should maybe be based off of distance not time? https://github.com/usdot-fhwa-stol/carma-platform/issues/1107 + RCLCPP_DEBUG_STREAM(get_logger(), "isexpired:" << isexpired); // TODO: temporary disabling expiration check - return false;// TODO maneuver expiration should maybe be based off of distance not time? https://github.com/usdot-fhwa-stol/carma-platform/issues/1107 + return false; } - - cav_srvs::PlanTrajectory PlanDelegator::composePlanTrajectoryRequest(const cav_msgs::TrajectoryPlan& latest_trajectory_plan, const uint16_t& current_maneuver_index) const + + std::shared_ptr PlanDelegator::composePlanTrajectoryRequest(const carma_planning_msgs::msg::TrajectoryPlan& latest_trajectory_plan, const uint16_t& current_maneuver_index) const { - auto plan_req = cav_srvs::PlanTrajectory{}; - plan_req.request.maneuver_plan = latest_maneuver_plan_; + auto plan_req = std::make_shared(); + plan_req->maneuver_plan = latest_maneuver_plan_; + // set current vehicle state if we have NOT planned any previous trajectories if(latest_trajectory_plan.trajectory_points.empty()) { - plan_req.request.header.stamp = latest_pose_.header.stamp; - plan_req.request.vehicle_state.longitudinal_vel = latest_twist_.twist.linear.x; - plan_req.request.vehicle_state.x_pos_global = latest_pose_.pose.position.x; - plan_req.request.vehicle_state.y_pos_global = latest_pose_.pose.position.y; + plan_req->header.stamp = latest_pose_.header.stamp; + plan_req->vehicle_state.longitudinal_vel = latest_twist_.twist.linear.x; + plan_req->vehicle_state.x_pos_global = latest_pose_.pose.position.x; + plan_req->vehicle_state.y_pos_global = latest_pose_.pose.position.y; double roll, pitch, yaw; carma_wm::geometry::rpyFromQuaternion(latest_pose_.pose.orientation, roll, pitch, yaw); - plan_req.request.vehicle_state.orientation = yaw; - plan_req.request.maneuver_index_to_plan = current_maneuver_index; + plan_req->vehicle_state.orientation = yaw; + plan_req->maneuver_index_to_plan = current_maneuver_index; } // set vehicle state based on last two planned trajectory points else { - cav_msgs::TrajectoryPlanPoint last_point = latest_trajectory_plan.trajectory_points.back(); - cav_msgs::TrajectoryPlanPoint second_last_point = *(latest_trajectory_plan.trajectory_points.rbegin() + 1); - plan_req.request.vehicle_state.x_pos_global = last_point.x; - plan_req.request.vehicle_state.y_pos_global = last_point.y; + carma_planning_msgs::msg::TrajectoryPlanPoint last_point = latest_trajectory_plan.trajectory_points.back(); + carma_planning_msgs::msg::TrajectoryPlanPoint second_last_point = *(latest_trajectory_plan.trajectory_points.rbegin() + 1); + plan_req->vehicle_state.x_pos_global = last_point.x; + plan_req->vehicle_state.y_pos_global = last_point.y; auto distance_diff = std::sqrt(std::pow(last_point.x - second_last_point.x, 2) + std::pow(last_point.y - second_last_point.y, 2)); - ros::Duration time_diff = last_point.target_time - second_last_point.target_time; - auto time_diff_sec = time_diff.toSec(); - plan_req.request.maneuver_index_to_plan = current_maneuver_index; + rclcpp::Duration time_diff = rclcpp::Time(last_point.target_time) - rclcpp::Time(second_last_point.target_time); + auto time_diff_sec = time_diff.seconds(); + plan_req->maneuver_index_to_plan = current_maneuver_index; // this assumes the vehicle does not have significant lateral velocity - plan_req.request.header.stamp = latest_trajectory_plan.trajectory_points.back().target_time; - plan_req.request.vehicle_state.longitudinal_vel = distance_diff / time_diff_sec; + plan_req->header.stamp = latest_trajectory_plan.trajectory_points.back().target_time; + plan_req->vehicle_state.longitudinal_vel = distance_diff / time_diff_sec; // TODO develop way to set yaw value for future points } return plan_req; } - bool PlanDelegator::isTrajectoryLongEnough(const cav_msgs::TrajectoryPlan& plan) const noexcept + bool PlanDelegator::isTrajectoryLongEnough(const carma_planning_msgs::msg::TrajectoryPlan& plan) const noexcept { - ros::Duration time_diff = plan.trajectory_points.back().target_time - plan.trajectory_points.front().target_time; - return time_diff.toSec() >= max_trajectory_duration_; + rclcpp::Duration time_diff = rclcpp::Time(plan.trajectory_points.back().target_time) - rclcpp::Time(plan.trajectory_points.front().target_time); + return time_diff.seconds() >= config_.max_trajectory_duration; } - void PlanDelegator::updateManeuverParameters(cav_msgs::Maneuver& maneuver) + void PlanDelegator::updateManeuverParameters(carma_planning_msgs::msg::Maneuver& maneuver) { + if (!wm_->getMap()) + { + RCLCPP_ERROR_STREAM(get_logger(), "Map is not set yet"); + return; + } // Update maneuver starting and ending downtrack distances double original_start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist); double original_end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist); - ROS_DEBUG_STREAM("Changing maneuver distances for planner: " << GET_MANEUVER_PROPERTY(maneuver, parameters.planning_tactical_plugin)); + RCLCPP_DEBUG_STREAM(get_logger(),"Changing maneuver distances for planner: " << GET_MANEUVER_PROPERTY(maneuver, parameters.planning_tactical_plugin)); double adjusted_start_dist = original_start_dist - length_to_front_bumper_; - ROS_DEBUG_STREAM("original_start_dist:" << original_start_dist); - ROS_DEBUG_STREAM("adjusted_start_dist:" << adjusted_start_dist); + RCLCPP_DEBUG_STREAM(get_logger(),"original_start_dist:" << original_start_dist); + RCLCPP_DEBUG_STREAM(get_logger(),"adjusted_start_dist:" << adjusted_start_dist); double adjusted_end_dist = original_end_dist - length_to_front_bumper_; - ROS_DEBUG_STREAM("original_end_dist:" << original_end_dist); - ROS_DEBUG_STREAM("adjusted_end_dist:" << adjusted_end_dist); + RCLCPP_DEBUG_STREAM(get_logger(),"original_end_dist:" << original_end_dist); + RCLCPP_DEBUG_STREAM(get_logger(),"adjusted_end_dist:" << adjusted_end_dist); SET_MANEUVER_PROPERTY(maneuver, start_dist, adjusted_start_dist); SET_MANEUVER_PROPERTY(maneuver, end_dist, adjusted_end_dist); @@ -292,7 +310,7 @@ namespace plan_delegator // Update maneuver-specific lanelet ID parameters // Note: Assumes that the maneuver start and end distances are adjusted by a distance less than the length of a lanelet. - if(maneuver.type == cav_msgs::Maneuver::LANE_FOLLOWING && !maneuver.lane_following_maneuver.lane_ids.empty()) + if(maneuver.type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING && !maneuver.lane_following_maneuver.lane_ids.empty()) { // Obtain the original starting lanelet from the maneuver's lane_ids lanelet::Id original_starting_lanelet_id = std::stoi(maneuver.lane_following_maneuver.lane_ids.front()); @@ -310,7 +328,7 @@ namespace plan_delegator // Lanelet preceeding the original starting lanelet is crossed by the updated maneuver, so it is added to the beginning of lane_ids if (starting_relation == lanelet::routing::RelationType::Successor && !found_lanelet_before_starting_lanelet) { - ROS_DEBUG_STREAM("Lanelet " << lanelet.id() << " inserted at the front of maneuver's lane_ids"); + RCLCPP_DEBUG_STREAM(get_logger(),"Lanelet " << lanelet.id() << " inserted at the front of maneuver's lane_ids"); // lane_ids array is ordered by increasing downtrack, so this new starting lanelet is inserted at the front maneuver.lane_following_maneuver.lane_ids.insert(maneuver.lane_following_maneuver.lane_ids.begin(), std::to_string(lanelet.id())); @@ -324,13 +342,13 @@ namespace plan_delegator // If the updated maneuver does not cross the original ending lanelet, remove that lanelet from the end of the maneuver's lane_ids if (!crosses_original_ending_lanelet) { - ROS_DEBUG_STREAM("Original ending lanelet " << original_ending_lanelet.id() << " removed from lane_ids since the updated maneuver no longer crosses it"); + RCLCPP_DEBUG_STREAM(get_logger(),"Original ending lanelet " << original_ending_lanelet.id() << " removed from lane_ids since the updated maneuver no longer crosses it"); // lane_ids array is ordered by increasing downtrack, so the last element in the array corresponds to the original ending lanelet maneuver.lane_following_maneuver.lane_ids.pop_back(); } } - else if (maneuver.type != cav_msgs::Maneuver::LANE_FOLLOWING) + else if (maneuver.type != carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING) { // Obtain the original starting lanelet from the maneuver lanelet::Id original_starting_lanelet_id = std::stoi(getManeuverStartingLaneletId(maneuver)); @@ -379,12 +397,12 @@ namespace plan_delegator } - cav_msgs::TrajectoryPlan PlanDelegator::planTrajectory() + carma_planning_msgs::msg::TrajectoryPlan PlanDelegator::planTrajectory() { - cav_msgs::TrajectoryPlan latest_trajectory_plan; + carma_planning_msgs::msg::TrajectoryPlan latest_trajectory_plan; if(!guidance_engaged) { - ROS_INFO_STREAM("Guidance is not engaged. Plan delegator will not plan trajectory."); + RCLCPP_INFO_STREAM(get_logger(),"Guidance is not engaged. Plan delegator will not plan trajectory."); return latest_trajectory_plan; } @@ -401,23 +419,23 @@ namespace plan_delegator auto& maneuver = latest_maneuver_plan_.maneuvers[current_maneuver_index]; // ignore expired maneuvers - if(isManeuverExpired(maneuver)) + if(isManeuverExpired(maneuver, get_clock()->now())) { - ROS_INFO_STREAM("Dropping expired maneuver: " << GET_MANEUVER_PROPERTY(maneuver, parameters.maneuver_id)); + RCLCPP_INFO_STREAM(get_logger(),"Dropping expired maneuver: " << GET_MANEUVER_PROPERTY(maneuver, parameters.maneuver_id)); // Update the maneuver plan index for the next loop ++current_maneuver_index; continue; } lanelet::BasicPoint2d current_loc(latest_pose_.pose.position.x, latest_pose_.pose.position.y); double current_downtrack = wm_->routeTrackPos(current_loc).downtrack; - ROS_DEBUG_STREAM("current_downtrack" << current_downtrack); + RCLCPP_DEBUG_STREAM(get_logger(),"current_downtrack" << current_downtrack); double maneuver_end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist); - ROS_DEBUG_STREAM("maneuver_end_dist" << maneuver_end_dist); + RCLCPP_DEBUG_STREAM(get_logger(),"maneuver_end_dist" << maneuver_end_dist); // ignore maneuver that is passed. if (current_downtrack > maneuver_end_dist) { - ROS_INFO_STREAM("Dropping passed maneuver: " << GET_MANEUVER_PROPERTY(maneuver, parameters.maneuver_id)); + RCLCPP_INFO_STREAM(get_logger(),"Dropping passed maneuver: " << GET_MANEUVER_PROPERTY(maneuver, parameters.maneuver_id)); // Update the maneuver plan index for the next loop ++current_maneuver_index; continue; @@ -428,57 +446,62 @@ namespace plan_delegator auto maneuver_planner = GET_MANEUVER_PROPERTY(maneuver, parameters.planning_tactical_plugin); auto client = getPlannerClientByName(maneuver_planner); - ROS_DEBUG_STREAM("Current planner: " << maneuver_planner); + RCLCPP_DEBUG_STREAM(get_logger(),"Current planner: " << maneuver_planner); // compose service request auto plan_req = composePlanTrajectoryRequest(latest_trajectory_plan, current_maneuver_index); + + auto plan_response = client->async_send_request(plan_req); + + auto future_status = plan_response.wait_for(std::chrono::milliseconds(100)); - if(client.call(plan_req)) + // Wait for the result. + if (future_status == std::future_status::ready) { // validate trajectory before add to the plan - if(!isTrajectoryValid(plan_req.response.trajectory_plan)) + if(!isTrajectoryValid(plan_response.get()->trajectory_plan)) { - ROS_WARN_STREAM("Found invalid trajectory with less than 2 trajectory points for " << latest_maneuver_plan_.maneuver_plan_id); + RCLCPP_WARN_STREAM(get_logger(),"Found invalid trajectory with less than 2 trajectory points for " << latest_maneuver_plan_.maneuver_plan_id); break; } //Remove duplicate point from start of trajectory if(latest_trajectory_plan.trajectory_points.size() !=0){ - if(latest_trajectory_plan.trajectory_points.back().target_time == plan_req.response.trajectory_plan.trajectory_points.front().target_time){ - ROS_DEBUG_STREAM("Removing duplicate point for planner: " << maneuver_planner); - plan_req.response.trajectory_plan.trajectory_points.erase(plan_req.response.trajectory_plan.trajectory_points.begin()); - ROS_DEBUG_STREAM("plan_req.response.trajectory_plan size: " << plan_req.response.trajectory_plan.trajectory_points.size()); + if(latest_trajectory_plan.trajectory_points.back().target_time == plan_response.get()->trajectory_plan.trajectory_points.front().target_time){ + RCLCPP_DEBUG_STREAM(get_logger(),"Removing duplicate point for planner: " << maneuver_planner); + plan_response.get()->trajectory_plan.trajectory_points.erase(plan_response.get()->trajectory_plan.trajectory_points.begin()); + RCLCPP_DEBUG_STREAM(get_logger(),"plan_response.get()->trajectory_plan size: " << plan_response.get()->trajectory_plan.trajectory_points.size()); } } latest_trajectory_plan.trajectory_points.insert(latest_trajectory_plan.trajectory_points.end(), - plan_req.response.trajectory_plan.trajectory_points.begin(), - plan_req.response.trajectory_plan.trajectory_points.end()); - ROS_DEBUG_STREAM("new latest_trajectory_plan size: " << latest_trajectory_plan.trajectory_points.size()); + plan_response.get()->trajectory_plan.trajectory_points.begin(), + plan_response.get()->trajectory_plan.trajectory_points.end()); + RCLCPP_DEBUG_STREAM(get_logger(),"new latest_trajectory_plan size: " << latest_trajectory_plan.trajectory_points.size()); // Assign the trajectory plan's initial longitudinal velocity based on the first tactical plugin's response if(first_trajectory_plan == true) { - latest_trajectory_plan.initial_longitudinal_velocity = plan_req.response.trajectory_plan.initial_longitudinal_velocity; + latest_trajectory_plan.initial_longitudinal_velocity = plan_response.get()->trajectory_plan.initial_longitudinal_velocity; first_trajectory_plan = false; } if(isTrajectoryLongEnough(latest_trajectory_plan)) { - ROS_INFO_STREAM("Plan Trajectory completed for " << latest_maneuver_plan_.maneuver_plan_id); + RCLCPP_INFO_STREAM(get_logger(),"Plan Trajectory completed for " << latest_maneuver_plan_.maneuver_plan_id); break; } // Update the maneuver plan index based on the last maneuver index converted to a trajectory // This is required since inlanecruising_plugin can plan a trajectory over contiguous LANE_FOLLOWING maneuvers - if(plan_req.response.related_maneuvers.size() > 0) + if(plan_response.get()->related_maneuvers.size() > 0) { - current_maneuver_index = plan_req.response.related_maneuvers.back() + 1; + current_maneuver_index = plan_response.get()->related_maneuvers.back() + 1; } } else { - ROS_WARN_STREAM("Unsuccessful service call to trajectory planner:" << maneuver_planner << " for plan ID " << latest_maneuver_plan_.maneuver_plan_id); + RCLCPP_WARN_STREAM(get_logger(),"Unsuccessful service call to trajectory planner:" << maneuver_planner << " for plan ID " << latest_maneuver_plan_.maneuver_plan_id); // if one service call fails, it should end plan immediately because it is there is no point to generate plan with empty space break; } @@ -486,20 +509,20 @@ namespace plan_delegator return latest_trajectory_plan; } - - void PlanDelegator::onTrajPlanTick(const ros::TimerEvent& te) + + void PlanDelegator::onTrajPlanTick() { - cav_msgs::TrajectoryPlan trajectory_plan = planTrajectory(); + carma_planning_msgs::msg::TrajectoryPlan trajectory_plan = planTrajectory(); // Check if planned trajectory is valid before send out if(isTrajectoryValid(trajectory_plan)) { - trajectory_plan.header.stamp = ros::Time::now(); - traj_pub_.publish(trajectory_plan); + trajectory_plan.header.stamp = get_clock()->now(); + traj_pub_->publish(trajectory_plan); } else { - ROS_WARN_STREAM("Planned trajectory is empty. It will not be published!"); + RCLCPP_WARN_STREAM(get_logger(),"Planned trajectory is empty. It will not be published!"); } } @@ -509,15 +532,21 @@ namespace plan_delegator tf2_buffer_.setUsingDedicatedThread(true); try { - geometry_msgs::TransformStamped tf = tf2_buffer_.lookupTransform("base_link", "vehicle_front", ros::Time(0), ros::Duration(20.0)); //save to local copy of transform 20 sec timeout + geometry_msgs::msg::TransformStamped tf = tf2_buffer_.lookupTransform("base_link", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(20.0, 0)); //save to local copy of transform 20 sec timeout length_to_front_bumper_ = tf.transform.translation.x; - ROS_DEBUG_STREAM("length_to_front_bumper_: " << length_to_front_bumper_); + RCLCPP_DEBUG_STREAM(get_logger(),"length_to_front_bumper_: " << length_to_front_bumper_); } catch (const tf2::TransformException &ex) { - ROS_WARN("%s", ex.what()); + RCLCPP_WARN_STREAM(get_logger(), ex.what()); } } -} +} // namespace plan_delegator + + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(plan_delegator::PlanDelegator) diff --git a/plan_delegator/src/plan_delegator_node.cpp b/plan_delegator/src/plan_delegator_node.cpp index b203cff6f1..6ce43c88f9 100644 --- a/plan_delegator/src/plan_delegator_node.cpp +++ b/plan_delegator/src/plan_delegator_node.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -14,15 +14,21 @@ * the License. */ -#include "ros/ros.h" +#include "rclcpp/rclcpp.hpp" #include "plan_delegator.hpp" // Main entry point for plan delegator int main(int argc, char** argv) { - ros::init(argc, argv, "plan_delegator"); - plan_delegator::PlanDelegator pd; - pd.init(); - pd.run(); - return 0; + 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; } \ No newline at end of file diff --git a/plan_delegator/test/plan_delegator.test b/plan_delegator/test/plan_delegator.test deleted file mode 100755 index 140a2d43e0..0000000000 --- a/plan_delegator/test/plan_delegator.test +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - diff --git a/plan_delegator/test/test_plan_delegator.cpp b/plan_delegator/test/test_plan_delegator.cpp index 65ba39919d..74b79c9c8b 100644 --- a/plan_delegator/test/test_plan_delegator.cpp +++ b/plan_delegator/test/test_plan_delegator.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * 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 @@ -16,142 +16,141 @@ #include #include -#include -#include +#include +#include #include -#include +#include #include "plan_delegator.hpp" class PlanDelegatorTest : public plan_delegator::PlanDelegator { public: + explicit PlanDelegatorTest(const rclcpp::NodeOptions &options) : plan_delegator::PlanDelegator(options){}; // Helper functions for unit test std::string getPlanningTopicPrefix() { - return this->planning_topic_prefix_; + return this->config_.planning_topic_prefix; } void setPlanningTopicPrefix(std::string prefix) { - this->planning_topic_prefix_ = prefix; + this->config_.planning_topic_prefix = prefix; } std::string getPlanningTopicSuffix() { - return this->planning_topic_suffix_; + return this->config_.planning_topic_suffix; } void setPlanningTopicSuffix(std::string suffix) { - this->planning_topic_suffix_ = suffix; + this->config_.planning_topic_suffix = suffix; } double getSpinRate() { - return this->trajectory_planning_rate_; + return this->config_.trajectory_planning_rate; } double getMaxTrajDuration() { - return this->max_trajectory_duration_; + return this->config_.max_trajectory_duration; } - cav_msgs::ManeuverPlan getLatestManeuverPlan() + carma_planning_msgs::msg::ManeuverPlan getLatestManeuverPlan() { return this->latest_maneuver_plan_; } - std::unordered_map getServiceMap() + std::unordered_map> getServiceMap() { return this->trajectory_planners_; } }; TEST(TestPlanDelegator, UnitTestPlanDelegator) { - PlanDelegatorTest pd; + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + auto pd = std::make_shared(options); + rclcpp_lifecycle::State dummy; + pd->handle_on_configure(dummy); + pd->handle_on_activate(dummy); // test initialization - EXPECT_EQ(0, pd.getPlanningTopicPrefix().compare("")); - EXPECT_EQ(0, pd.getPlanningTopicSuffix().compare("")); - EXPECT_EQ(10.0, pd.getSpinRate()); - EXPECT_EQ(6.0, pd.getMaxTrajDuration()); + EXPECT_EQ(0, pd->getPlanningTopicPrefix().compare("/plugins/")); + EXPECT_EQ(0, pd->getPlanningTopicSuffix().compare("/plan_trajectory")); + EXPECT_EQ(10.0, pd->getSpinRate()); + EXPECT_EQ(6.0, pd->getMaxTrajDuration()); // test maneuver plan callback - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver maneuver; + carma_planning_msgs::msg::ManeuverPlan plan; + carma_planning_msgs::msg::Maneuver maneuver; maneuver.type = maneuver.LANE_FOLLOWING; maneuver.lane_following_maneuver.parameters.planning_strategic_plugin = "plugin_A"; plan.maneuvers.push_back(maneuver); - pd.maneuverPlanCallback(cav_msgs::ManeuverPlanConstPtr(new cav_msgs::ManeuverPlan(plan))); - EXPECT_EQ("plugin_A", GET_MANEUVER_PROPERTY(pd.getLatestManeuverPlan().maneuvers[0], parameters.planning_strategic_plugin)); - cav_msgs::ManeuverPlan new_plan; - pd.maneuverPlanCallback(cav_msgs::ManeuverPlanConstPtr(new cav_msgs::ManeuverPlan(new_plan))); + pd->maneuverPlanCallback(std::make_unique(plan)); + EXPECT_EQ("plugin_A", GET_MANEUVER_PROPERTY(pd->getLatestManeuverPlan().maneuvers[0], parameters.planning_strategic_plugin)); + carma_planning_msgs::msg::ManeuverPlan new_plan; + pd->maneuverPlanCallback(std::make_unique(new_plan)); // empty plan should not be stored locally - EXPECT_EQ("plugin_A", GET_MANEUVER_PROPERTY(pd.getLatestManeuverPlan().maneuvers[0], parameters.planning_strategic_plugin)); + EXPECT_EQ("plugin_A", GET_MANEUVER_PROPERTY(pd->getLatestManeuverPlan().maneuvers[0], parameters.planning_strategic_plugin)); // test create service client - EXPECT_THROW(pd.getPlannerClientByName(""), std::invalid_argument); - pd.setPlanningTopicPrefix("/guidance/plugins/"); - pd.setPlanningTopicSuffix("/plan_trajectory"); - ros::ServiceClient plugin_A = pd.getPlannerClientByName("plugin_A"); - EXPECT_EQ("/guidance/plugins/plugin_A/plan_trajectory", plugin_A.getService()); - EXPECT_EQ(1, pd.getServiceMap().size()); - ros::ServiceClient plugin_A_copy = pd.getPlannerClientByName("plugin_A"); + EXPECT_THROW(pd->getPlannerClientByName(""), std::invalid_argument); + pd->setPlanningTopicPrefix("/guidance/plugins/"); + pd->setPlanningTopicSuffix("/plan_trajectory"); + auto plugin_A = pd->getPlannerClientByName("plugin_A"); + EXPECT_EQ(0, std::string(plugin_A->get_service_name()).compare("/guidance/plugins/plugin_A/plan_trajectory")); + EXPECT_EQ(1, pd->getServiceMap().size()); + auto plugin_A_copy = pd->getPlannerClientByName("plugin_A"); EXPECT_EQ(true, plugin_A == plugin_A_copy); // test expired maneuver - ros::Time test_time(0, 1000); - cav_msgs::Maneuver test_maneuver; - test_maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING; + rclcpp::Time test_time(0, 1000); + carma_planning_msgs::msg::Maneuver test_maneuver; + test_maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; test_maneuver.lane_following_maneuver.end_time = test_time; - EXPECT_EQ(true, pd.isManeuverExpired(test_maneuver)); - ros::Time test_time_eariler(0, 500); - EXPECT_EQ(false, pd.isManeuverExpired(test_maneuver, test_time_eariler)); + EXPECT_EQ(true, pd->isManeuverExpired(test_maneuver, pd->get_clock()->now())); + rclcpp::Time test_time_earlier(0, 500, pd->get_clock()->get_clock_type()); + EXPECT_EQ(false, pd->isManeuverExpired(test_maneuver, test_time_earlier)); // test compose new plan trajectory request uint16_t current_maneuver_index = 0; - cav_msgs::TrajectoryPlan traj_plan; - cav_msgs::TrajectoryPlanPoint point_1; + carma_planning_msgs::msg::TrajectoryPlan traj_plan; + carma_planning_msgs::msg::TrajectoryPlanPoint point_1; point_1.x = 0.0; point_1.y = 0.0; - point_1.target_time = ros::Time(0); - cav_msgs::TrajectoryPlanPoint point_2; + point_1.target_time = rclcpp::Time(0, 0, pd->get_clock()->get_clock_type()); + carma_planning_msgs::msg::TrajectoryPlanPoint point_2; point_2.x = 1.0; point_2.y = 1.0; - point_2.target_time = ros::Time(1.41421); + point_2.target_time = rclcpp::Time(1.41421e9, pd->get_clock()->get_clock_type()); traj_plan.trajectory_points.push_back(point_1); traj_plan.trajectory_points.push_back(point_2); - cav_srvs::PlanTrajectory req = pd.composePlanTrajectoryRequest(traj_plan, current_maneuver_index); - EXPECT_NEAR(1.0, req.request.vehicle_state.x_pos_global, 0.01); - EXPECT_NEAR(1.0, req.request.vehicle_state.y_pos_global, 0.01); - EXPECT_NEAR(1.0, req.request.vehicle_state.longitudinal_vel, 0.1); - EXPECT_EQ(0, req.request.maneuver_index_to_plan); + auto req = pd->composePlanTrajectoryRequest(traj_plan, current_maneuver_index); + EXPECT_NEAR(1.0, req->vehicle_state.x_pos_global, 0.01); + EXPECT_NEAR(1.0, req->vehicle_state.y_pos_global, 0.01); + EXPECT_NEAR(1.0, req->vehicle_state.longitudinal_vel, 0.1); + EXPECT_EQ(0, req->maneuver_index_to_plan); } TEST(TestPlanDelegator, TestPlanDelegator) { - ros::NodeHandle nh = ros::NodeHandle(); - cav_msgs::TrajectoryPlan res_plan; - // bool flag = false; - ros::Publisher maneuver_pub = nh.advertise("final_maneuver_plan", 5); - // ros::Subscriber traj_sub = nh.subscribe("plan_trajectory", 5, [&](cav_msgs::TrajectoryPlanConstPtr msg){ - // res_plan = msg.get(); - - // }); - // boost::function cb = [&](cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& res) -> bool - // { - // flag = true; - // // cav_msgs::TrajectoryPlan sending_plan; - // // sending_plan.trajectory_id = "plugin_A"; - // // res.trajectory_plan = sending_plan; - // return true; - // }; - //ros::ServiceServer plugin_A_server = nh.advertiseService("/guidance/plugins/plugin_A/plan_trajectory", cb); - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver maneuver; + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + auto pd = std::make_shared(options); + rclcpp_lifecycle::State dummy; + pd->handle_on_configure(dummy); + pd->handle_on_activate(dummy); + + carma_planning_msgs::msg::TrajectoryPlan res_plan; + + auto maneuver_pub = pd->create_publisher("final_maneuver_plan", 5); + + carma_planning_msgs::msg::ManeuverPlan plan; + carma_planning_msgs::msg::Maneuver maneuver; maneuver.type = maneuver.LANE_FOLLOWING; maneuver.lane_following_maneuver.parameters.planning_strategic_plugin = "plugin_A"; plan.maneuvers.push_back(maneuver); - maneuver_pub.publish(plan); - // std::string res = res_plan->trajectory_id; - // //EXPECT_EQ("plugin_A", res); + maneuver_pub->on_activate(); + maneuver_pub->publish(plan); std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - auto num = maneuver_pub.getNumSubscribers(); + auto num = maneuver_pub->get_subscription_count(); EXPECT_EQ(1, num); } @@ -159,10 +158,17 @@ * \brief Main entrypoint for unit tests */ int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_plan_delegator"); - //std::thread spinner([] {while (ros::ok()) ros::spin();}); - auto res = RUN_ALL_TESTS(); - //ros::shutdown(); - return res; + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + auto ret = rcutils_logging_set_logger_level( + rclcpp::get_logger("plan_delegator").get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; } From cd05f874ab72f4e41bb959a393f2e32ab20e9350 Mon Sep 17 00:00:00 2001 From: JonSmet <34752540+JonSmet@users.noreply.github.com> Date: Mon, 27 Jun 2022 16:40:37 -0400 Subject: [PATCH 026/165] Port guidance node to ROS2 (#1831) * Initial guidance ROS2 * Code cleanup * Remove unused boolean flag --- carma/launch/guidance.launch | 3 - carma/launch/guidance.launch.py | 20 +++ guidance/CMakeLists.txt | 128 +++++---------- guidance/README.md | 5 + guidance/config/parameters.yaml | 3 + guidance/config/params.yaml | 17 -- guidance/include/guidance/guidance_config.hpp | 42 +++++ .../guidance/guidance_state_machine.hpp | 42 +++-- guidance/include/guidance/guidance_worker.hpp | 155 +++++++++++------- guidance/launch/guidance_launch.py | 68 ++++++++ guidance/launch/guidance_main.launch | 27 --- guidance/package.xml | 51 ++++-- guidance/src/guidance/guidance_worker.cpp | 95 ----------- .../{guidance => }/guidance_state_machine.cpp | 45 +++-- guidance/src/guidance_worker.cpp | 153 +++++++++++++++++ .../{guidance/guidance_node.cpp => main.cpp} | 23 ++- guidance/test/guidance_launch.test | 20 --- guidance/test/guidance_node_test.cpp | 69 +++++--- guidance/test/guidance_test_suite.cpp | 61 +++++++ guidance/test/test_guidance_state_machine.cpp | 85 +++++----- 20 files changed, 683 insertions(+), 429 deletions(-) create mode 100644 guidance/README.md create mode 100644 guidance/config/parameters.yaml delete mode 100644 guidance/config/params.yaml create mode 100644 guidance/include/guidance/guidance_config.hpp create mode 100644 guidance/launch/guidance_launch.py delete mode 100644 guidance/launch/guidance_main.launch delete mode 100644 guidance/src/guidance/guidance_worker.cpp rename guidance/src/{guidance => }/guidance_state_machine.cpp (80%) create mode 100644 guidance/src/guidance_worker.cpp rename guidance/src/{guidance/guidance_node.cpp => main.cpp} (62%) delete mode 100644 guidance/test/guidance_launch.test create mode 100644 guidance/test/guidance_test_suite.cpp diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 413edfec3c..9a679f23b5 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -86,9 +86,6 @@ - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index 11b858f5ab..6603d86776 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -60,6 +60,9 @@ def generate_launch_description(): route_param_file = os.path.join( get_package_share_directory('route'), 'config/parameters.yaml') + + guidance_param_file = os.path.join( + get_package_share_directory('guidance'), 'config/parameters.yaml') plan_delegator_param_file = os.path.join( get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml') @@ -160,6 +163,23 @@ def generate_launch_description(): route_param_file, vehicle_config_param_file ] + ), + ComposableNode( + package='guidance', + plugin='guidance::GuidanceWorker', + name='guidance_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('route', env_log_levels) } + ], + remappings = [ + ("vehicle_status", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle_status" ] ), + ("robot_status", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/robot_status" ] ), + ("enable_robotic", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/enable_robotic" ] ), + ], + parameters=[ + guidance_param_file + ] ) ] ) diff --git a/guidance/CMakeLists.txt b/guidance/CMakeLists.txt index cef90f48ab..54c53962fc 100644 --- a/guidance/CMakeLists.txt +++ b/guidance/CMakeLists.txt @@ -1,4 +1,5 @@ -# Copyright (C) 2018-2021 LEIDOS. + +# Copyright (C) 2018-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 @@ -12,111 +13,72 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(guidance) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - carma_utils - cav_msgs - cav_srvs - roscpp - std_msgs - autoware_msgs -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES ui_integration - CATKIN_DEPENDS carma_utils cav_msgs cav_srvs roscpp std_msgs autoware_msgs -# DEPENDS system_lib -) +# Name build targets +set(state_lib guidance_state_lib) +set(node_lib guidance_node) +set(node_exec guidance_node_exec) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} ) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -file(GLOB_RECURSE headers */*.hpp */*.h) -add_executable(${PROJECT_NAME}_node - src/guidance/guidance_node.cpp - src/guidance/guidance_worker.cpp - src/guidance/guidance_state_machine.cpp +# Build +ament_auto_add_library(${state_lib} + src/guidance_state_machine.cpp ) -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_library(guidance_state_machine_library src/guidance/guidance_state_machine.cpp) -add_dependencies(guidance_state_machine_library ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(guidance_state_machine_library - ${catkin_LIBRARIES} +ament_auto_add_library(${node_lib} SHARED + src/guidance_worker.cpp ) -target_link_libraries(${PROJECT_NAME}_node - guidance_state_machine_library + +ament_auto_add_executable(${node_exec} + src/main.cpp ) -############# -## Install ## -############# +# Register component +rclcpp_components_register_nodes(${node_lib} "guidance::GuidanceWorker") -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME}_node guidance_state_machine_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_lib} + ${state_lib} ) -# Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE +target_link_libraries(${node_exec} + ${node_lib} ) -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -############# -## Testing ## -############# + ament_add_gtest(test_guidance + test/guidance_node_test.cpp + test/test_guidance_state_machine.cpp + ) -catkin_add_gmock(${PROJECT_NAME}-test test/test_guidance_state_machine.cpp) -target_link_libraries(${PROJECT_NAME}-test guidance_state_machine_library ${catkin_LIBRARIES}) + ament_target_dependencies(test_guidance ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_guidance ${node_lib}) -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(guidance_test test/guidance_launch.test test/guidance_node_test.cpp) - target_link_libraries(guidance_test ${catkin_LIBRARIES}) endif() +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/guidance/README.md b/guidance/README.md new file mode 100644 index 0000000000..b159cd0ed4 --- /dev/null +++ b/guidance/README.md @@ -0,0 +1,5 @@ +# guidance + +The Guidance node is a ROS node is responsible for guidance state machine management and for providing a pass-through service to invoke the system enable command from the UI. + +Link to detailed design document on Confluence: [Click Here](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/1309442090/Detailed+Design+-+Guidance) \ No newline at end of file diff --git a/guidance/config/parameters.yaml b/guidance/config/parameters.yaml new file mode 100644 index 0000000000..099feb0a9e --- /dev/null +++ b/guidance/config/parameters.yaml @@ -0,0 +1,3 @@ +# Double: The rate at which the Guidance Node will process message +# Units: hz +spin_rate_hz: 10.0 diff --git a/guidance/config/params.yaml b/guidance/config/params.yaml deleted file mode 100644 index 4368b6e98c..0000000000 --- a/guidance/config/params.yaml +++ /dev/null @@ -1,17 +0,0 @@ -# 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. - -# Integer: The rate at which the Guidance Node will process message -# Units: hz -spin_rate_hz: 10 diff --git a/guidance/include/guidance/guidance_config.hpp b/guidance/include/guidance/guidance_config.hpp new file mode 100644 index 0000000000..0b715f5a7f --- /dev/null +++ b/guidance/include/guidance/guidance_config.hpp @@ -0,0 +1,42 @@ +#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. + */ + +#include +#include + +namespace guidance +{ + + /** + * \brief Stuct containing the algorithm configuration values for GuidanceWorker + */ + struct Config + { + double spin_rate_hz = 10.0; // (Hz) The rate at which the Guidance Node will process messages + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "guidance::Config { " << std::endl + << "spin_rate_hz: " << c.spin_rate_hz << std::endl + << "}" << std::endl; + return output; + } + }; + +} // guidance \ No newline at end of file diff --git a/guidance/include/guidance/guidance_state_machine.hpp b/guidance/include/guidance/guidance_state_machine.hpp index 1b1ac0f118..7cb8b5ce75 100644 --- a/guidance/include/guidance/guidance_state_machine.hpp +++ b/guidance/include/guidance/guidance_state_machine.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-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 @@ -16,11 +16,12 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include +#include +#include + +#include namespace guidance { @@ -51,19 +52,25 @@ namespace guidance }; /*! - * \brief Default constructor for GuidanceStateMachine + * \brief constructor for GuidanceStateMachine + * \param logger The logger interface that will be used by this object */ - GuidanceStateMachine() = default; + GuidanceStateMachine(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger); /*! * \brief Handle vehicle status message from ROS network. */ - void onVehicleStatus(const autoware_msgs::VehicleStatusConstPtr& msg); + void onVehicleStatus(autoware_msgs::msg::VehicleStatus::UniquePtr msg); + + /*! + * \brief Updates Guidance State Machine with INITIALIZED signal + */ + void onGuidanceInitialized(); /*! - * \brief Handle system_alert message from ROS network. + * \brief Updates Guidance State Machine with SHUTDOWN signal */ - void onSystemAlert(const cav_msgs::SystemAlertConstPtr& msg); + void onGuidanceShutdown(); /*! * \brief Handle set_guidance_active service call from ROS network. @@ -73,12 +80,12 @@ namespace guidance /*! * \brief Handle robotic_status message from ROS network. */ - void onRoboticStatus(const cav_msgs::RobotEnabledConstPtr& msg); + void onRoboticStatus(carma_driver_msgs::msg::RobotEnabled::UniquePtr msg); /*! * \brief Handle route event message. */ - void onRouteEvent(const cav_msgs::RouteEventConstPtr& msg); + void onRouteEvent(carma_planning_msgs::msg::RouteEvent::UniquePtr msg); /*! * \brief Indicate if SetEnableRobotic needs to be called in ACTIVE state. @@ -106,13 +113,12 @@ namespace guidance // make one service call in ACTIVE state to engage bool called_robotic_engage_in_active_{false}; - // Flag indicating that DRIVERS_READY signal was received during system startup. - // This is needed for state transitions since the most recent system alert message may contain unrelated information - bool operational_drivers_{false}; - // Current vehicle speed in m/s. Used to handle end of route state transition. double current_velocity_ = 0.0; + // Logger interface + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; + }; -} +} // guidance \ No newline at end of file diff --git a/guidance/include/guidance/guidance_worker.hpp b/guidance/include/guidance/guidance_worker.hpp index 96638f73e2..62c3ea7847 100644 --- a/guidance/include/guidance/guidance_worker.hpp +++ b/guidance/include/guidance/guidance_worker.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-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 @@ -16,64 +16,103 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include "guidance/guidance_state_machine.hpp" +#include +#include "guidance/guidance_config.hpp" + namespace guidance { - class GuidanceWorker - { - public: - /*! - * \brief Default constructor for GuidanceWorker - */ - GuidanceWorker() = default; - - /*! - * \brief Begin normal execution of Guidance worker. Will take over control flow of program and exit from here. - * - * \return The exit status of this program - */ - int run(); - - private: - - // spin callback function - bool spin_cb(); - - // Message/service callbacks - bool guidance_acivation_cb(cav_srvs::SetGuidanceActiveRequest& req, cav_srvs::SetGuidanceActiveResponse& res); - void route_event_cb(const cav_msgs::RouteEventConstPtr& msg); - void robot_status_cb(const cav_msgs::RobotEnabledConstPtr& msg); - void system_alert_cb(const cav_msgs::SystemAlertConstPtr& msg); - void vehicle_status_cb(const autoware_msgs::VehicleStatusConstPtr& msg); - - // Service servers - ros::ServiceServer guidance_activate_service_server_; - - // Publishers - ros::Publisher state_publisher_; - ros::ServiceClient enable_client_; - - // Subscribers - ros::Subscriber robot_status_subscriber_; - ros::Subscriber route_event_subscriber_; - ros::Subscriber vehicle_status_subscriber_; - - // Node handles - ros::CARMANodeHandle nh_{}, pnh_{"~"}; - - // Guidance state machine - GuidanceStateMachine gsm_; - }; -} \ No newline at end of file + + /** + * \brief Worker class for the Guidance node + * + */ + class GuidanceWorker : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Subscribers + carma_ros2_utils::SubPtr robot_status_subscriber_; + carma_ros2_utils::SubPtr route_event_subscriber_; + carma_ros2_utils::SubPtr vehicle_status_subscriber_; + + // Publishers + carma_ros2_utils::PubPtr state_publisher_; + + // Service Clients + carma_ros2_utils::ClientPtr enable_client_; + + // Service Servers + carma_ros2_utils::ServicePtr guidance_activate_service_server_; + + // Timers + rclcpp::TimerBase::SharedPtr spin_timer_; + + // Node configuration + Config config_; + + // Guidance state machine + GuidanceStateMachine gsm_; + + public: + /** + * \brief GuidanceWorker constructor + */ + explicit GuidanceWorker(const rclcpp::NodeOptions &); + + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief Timer callback + */ + bool spin_cb(); + + /** + * \brief Callback for route event messages + * \param msg Latest route event message + */ + void route_event_cb(carma_planning_msgs::msg::RouteEvent::UniquePtr msg); + + /** + * \brief Callback for robot enabled messages + * \param msg Latest robot enabled message + */ + void robot_status_cb(carma_driver_msgs::msg::RobotEnabled::UniquePtr msg); + + /** + * \brief Callback for vehicle status messages + * \param msg Latest vehicle status message + */ + void vehicle_status_cb(const autoware_msgs::msg::VehicleStatus::UniquePtr msg); + + /** + * \brief Set_guidance_active service callback. User can attempt to activate the guidance system by calling this service + * \param req A carma_planning_msgs::srv::SetGuidanceActive::Request msg + * \param resp A carma_planning_msgs::srv::SetGuidanceActive::Response msg + */ + bool guidance_activation_cb(const std::shared_ptr, + const std::shared_ptr req, + std::shared_ptr resp); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &prev_state); + }; + +} // guidance diff --git a/guidance/launch/guidance_launch.py b/guidance/launch/guidance_launch.py new file mode 100644 index 0000000000..ae57b98501 --- /dev/null +++ b/guidance/launch/guidance_launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA guidance_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('guidance'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='guidance_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='guidance', + plugin='guidance::GuidanceWorker', + name='guidance_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/guidance/launch/guidance_main.launch b/guidance/launch/guidance_main.launch deleted file mode 100644 index f62af549a0..0000000000 --- a/guidance/launch/guidance_main.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - diff --git a/guidance/package.xml b/guidance/package.xml index 0afb3413bd..911710c16c 100644 --- a/guidance/package.xml +++ b/guidance/package.xml @@ -1,16 +1,45 @@ + + + guidance - 3.3.0 - The Guidance package - rushk - Apache 2.0 License - catkin - carma_utils - cav_msgs - cav_srvs - roscpp - std_msgs - autoware_msgs + 4.0.0 + The guidance package + + carma + + Apache 2.0 + + ament_cmake carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + carma_planning_msgs + carma_driver_msgs + autoware_msgs + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + diff --git a/guidance/src/guidance/guidance_worker.cpp b/guidance/src/guidance/guidance_worker.cpp deleted file mode 100644 index 31540c6654..0000000000 --- a/guidance/src/guidance/guidance_worker.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* - * 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. - */ - -#include "guidance/guidance_worker.hpp" - -namespace guidance -{ - - void GuidanceWorker::vehicle_status_cb(const autoware_msgs::VehicleStatusConstPtr& msg) - { - gsm_.onVehicleStatus(msg); - } - - void GuidanceWorker::system_alert_cb(const cav_msgs::SystemAlertConstPtr& msg) - { - gsm_.onSystemAlert(msg); - } - - void GuidanceWorker::robot_status_cb(const cav_msgs::RobotEnabledConstPtr& msg) - { - gsm_.onRoboticStatus(msg); - } - - void GuidanceWorker::route_event_cb(const cav_msgs::RouteEventConstPtr& msg) - { - gsm_.onRouteEvent(msg); - } - - bool GuidanceWorker::guidance_acivation_cb(cav_srvs::SetGuidanceActiveRequest& req, cav_srvs::SetGuidanceActiveResponse& res) - { - // Translate message type from GuidanceActiveRequest to SetEnableRobotic - ROS_INFO_STREAM("Request for guidance activation recv'd with status " << req.guidance_active); - if(!req.guidance_active) - { - cav_srvs::SetEnableRobotic srv; - srv.request.set = cav_srvs::SetEnableRobotic::Request::DISABLE; - enable_client_.call(srv); - } - gsm_.onSetGuidanceActive(req.guidance_active); - res.guidance_status = (gsm_.getCurrentState() == GuidanceStateMachine::ACTIVE); - return true; - } - - bool GuidanceWorker::spin_cb() - { - if(gsm_.shouldCallSetEnableRobotic()) { - cav_srvs::SetEnableRobotic srv; - srv.request.set = cav_srvs::SetEnableRobotic::Request::ENABLE; - enable_client_.call(srv); - } - cav_msgs::GuidanceState state; - state.state = gsm_.getCurrentState(); - state_publisher_.publish(state); - return true; - } - - int GuidanceWorker::run() - { - ROS_INFO("Initalizing guidance node..."); - ros::CARMANodeHandle::setSystemAlertCallback(std::bind(&GuidanceWorker::system_alert_cb, this, std::placeholders::_1)); - // Init our ROS objects - guidance_activate_service_server_ = nh_.advertiseService("set_guidance_active", &GuidanceWorker::guidance_acivation_cb, this); - state_publisher_ = nh_.advertise("state", 5); - robot_status_subscriber_ = nh_.subscribe("robot_status", 5, &GuidanceWorker::robot_status_cb, this); - route_event_subscriber_ = nh_.subscribe("route_event", 5, &GuidanceWorker::route_event_cb, this); - vehicle_status_subscriber_ = nh_.subscribe("vehicle_status", 5, &GuidanceWorker::vehicle_status_cb, this); - enable_client_ = nh_.serviceClient("controller/enable_robotic"); - - // Load the spin rate param to determine how fast to process messages - // Default rate 10.0 Hz - double spin_rate = pnh_.param("spin_rate_hz", 10.0); - - // Spin until system shutdown - ROS_INFO_STREAM("Guidance node initialized, spinning at " << spin_rate << "hz..."); - ros::CARMANodeHandle::setSpinRate(spin_rate); - ros::CARMANodeHandle::setSpinCallback(std::bind(&GuidanceWorker::spin_cb, this)); - ros::CARMANodeHandle::spin(); - - // return - return 0; - } -} diff --git a/guidance/src/guidance/guidance_state_machine.cpp b/guidance/src/guidance_state_machine.cpp similarity index 80% rename from guidance/src/guidance/guidance_state_machine.cpp rename to guidance/src/guidance_state_machine.cpp index 8701b1136c..6cb7c9def7 100644 --- a/guidance/src/guidance/guidance_state_machine.cpp +++ b/guidance/src/guidance_state_machine.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-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 @@ -15,10 +15,12 @@ */ #include "guidance/guidance_state_machine.hpp" -#include namespace guidance { + GuidanceStateMachine::GuidanceStateMachine(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) + : logger_(logger) {} + void GuidanceStateMachine::onGuidanceSignal(Signal signal) { // Set to OFF state of SHUTDOWN signal received @@ -83,34 +85,29 @@ namespace guidance } } - void GuidanceStateMachine::onVehicleStatus(const autoware_msgs::VehicleStatusConstPtr& msg) + void GuidanceStateMachine::onVehicleStatus(autoware_msgs::msg::VehicleStatus::UniquePtr msg) { current_velocity_ = msg->speed * 0.277777; // Convert kilometers per hour to meters per second. Rounded down so that it comes under epsilon for parking check if (current_guidance_state_ == State::ENTER_PARK) { // '3' indicates vehicle gearshift is currently set to PARK - if(msg->current_gear.gear== autoware_msgs::Gear::PARK) + if(msg->current_gear.gear== autoware_msgs::msg::Gear::PARK) { onGuidanceSignal(Signal::OVERRIDE); // Required for ENTER_PARK -> INACTIVE - if(operational_drivers_){ - onGuidanceSignal(Signal::INITIALIZED); - } + onGuidanceSignal(Signal::INITIALIZED); // Required for INACTIVE -> DRIVERS_READY } } } - void GuidanceStateMachine::onSystemAlert(const cav_msgs::SystemAlertConstPtr& msg) + void GuidanceStateMachine::onGuidanceInitialized() { - if(msg->type == msg->DRIVERS_READY) - { - operational_drivers_ = true; - onGuidanceSignal(Signal::INITIALIZED); - } else if(msg->type == msg->SHUTDOWN) - { - operational_drivers_ = false; - onGuidanceSignal(Signal::SHUTDOWN); - } + onGuidanceSignal(Signal::INITIALIZED); + } + + void GuidanceStateMachine::onGuidanceShutdown() + { + onGuidanceSignal(Signal::SHUTDOWN); } void GuidanceStateMachine::onSetGuidanceActive(bool msg) @@ -125,7 +122,7 @@ namespace guidance } - void GuidanceStateMachine::onRoboticStatus(const cav_msgs::RobotEnabledConstPtr& msg) + void GuidanceStateMachine::onRoboticStatus(carma_driver_msgs::msg::RobotEnabled::UniquePtr msg) { // robotic status changes from false to true if(!robotic_active_status_ && msg->robot_active) @@ -141,17 +138,17 @@ namespace guidance } } - void GuidanceStateMachine::onRouteEvent(const cav_msgs::RouteEventConstPtr& msg) + void GuidanceStateMachine::onRouteEvent(carma_planning_msgs::msg::RouteEvent::UniquePtr msg) { - if(msg->event == cav_msgs::RouteEvent::ROUTE_DEPARTED || - msg->event == cav_msgs::RouteEvent::ROUTE_ABORTED) { + if(msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_DEPARTED || + msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_ABORTED) { onGuidanceSignal(Signal::DISENGAGED); } - else if(msg->event == cav_msgs::RouteEvent::ROUTE_COMPLETED){ + else if(msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED){ if (fabs(current_velocity_) < 0.001) { // Check we have successfully stopped onGuidanceSignal(Signal::PARK); // ENGAGED -> ENTER_PARK this state restricts transitioning out of ENTER_PARK until vehicle is shifted to PARK } else { // Vehicle was not able to stop transition to inactive - ROS_WARN_STREAM("Vehicle failed to park on route completion because current velocity was " << current_velocity_); + RCLCPP_WARN_STREAM(logger_->get_logger(), "Vehicle failed to park on route completion because current velocity was " << current_velocity_); onGuidanceSignal(Signal::OVERRIDE); } } @@ -179,4 +176,4 @@ namespace guidance return false; } -} +} \ No newline at end of file diff --git a/guidance/src/guidance_worker.cpp b/guidance/src/guidance_worker.cpp new file mode 100644 index 0000000000..f0c1c98a6a --- /dev/null +++ b/guidance/src/guidance_worker.cpp @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2018-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 "guidance/guidance_worker.hpp" + +namespace guidance +{ + namespace std_ph = std::placeholders; + + GuidanceWorker::GuidanceWorker(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options), + gsm_(this->get_node_logging_interface()) + { + // Create initial config + config_ = Config(); + + // Declare parameters + config_.spin_rate_hz = declare_parameter("spin_rate_hz", config_.spin_rate_hz); + } + + rcl_interfaces::msg::SetParametersResult GuidanceWorker::parameter_update_callback(const std::vector ¶meters) + { + auto error = update_params({{"spin_rate_hz", config_.spin_rate_hz}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error; + + return result; + } + + carma_ros2_utils::CallbackReturn GuidanceWorker::handle_on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO_STREAM(get_logger(), "GuidanceWorker trying to configure"); + + // Reset config + config_ = Config(); + + // Load parameters + get_parameter("spin_rate_hz", config_.spin_rate_hz); + + RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&GuidanceWorker::parameter_update_callback, this, std_ph::_1)); + + // Setup subscribers + robot_status_subscriber_ = create_subscription("robot_status", 5, + std::bind(&GuidanceWorker::robot_status_cb, this, std_ph::_1)); + route_event_subscriber_ = create_subscription("route_event", 5, + std::bind(&GuidanceWorker::route_event_cb, this, std_ph::_1)); + vehicle_status_subscriber_ = create_subscription("vehicle_status", 5, + std::bind(&GuidanceWorker::vehicle_status_cb, this, std_ph::_1)); + + // Setup publishers + state_publisher_ = create_publisher("state", 5); + + // Setup service clients + enable_client_ = create_client("enable_robotic"); + + // Setup service servers + guidance_activate_service_server_ = create_service("set_guidance_active", + std::bind(&GuidanceWorker::guidance_activation_cb, this, std_ph::_1, std_ph::_2, std_ph::_3)); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn GuidanceWorker::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + // Setup timer + auto guidance_spin_period_ms = int ((1 / config_.spin_rate_hz) * 1000); // Conversion rom frequency (Hz) to milliseconds time period + spin_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(guidance_spin_period_ms), + std::bind(&GuidanceWorker::spin_cb, this)); + + // Update Guidance State Machine since node activation by the lifecycle system indicates that all required drivers are ready + gsm_.onGuidanceInitialized(); + + return CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn GuidanceWorker::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + // Update Guidance State Machine with SHUTDOWN signal + gsm_.onGuidanceShutdown(); + } + + bool GuidanceWorker::guidance_activation_cb(const std::shared_ptr, + const std::shared_ptr req, + std::shared_ptr resp) + { + // Translate message type from GuidanceActiveRequest to SetEnableRobotic + if(!req->guidance_active) + { + auto enable_robotic_request = std::make_shared(); + enable_robotic_request->set = carma_driver_msgs::srv::SetEnableRobotic::Request::DISABLE; + enable_client_->async_send_request(enable_robotic_request); + } + + gsm_.onSetGuidanceActive(req->guidance_active); + resp->guidance_status = (gsm_.getCurrentState() == GuidanceStateMachine::ACTIVE); + return true; + } + + bool GuidanceWorker::spin_cb() + { + if(gsm_.shouldCallSetEnableRobotic()) { + auto req = std::make_shared(); + req->set = carma_driver_msgs::srv::SetEnableRobotic::Request::ENABLE; + enable_client_->async_send_request(req); + } + + carma_planning_msgs::msg::GuidanceState state; + state.state = gsm_.getCurrentState(); + state_publisher_->publish(state); + return true; + } + + void GuidanceWorker::route_event_cb(carma_planning_msgs::msg::RouteEvent::UniquePtr msg) + { + gsm_.onRouteEvent(move(msg)); + } + + void GuidanceWorker::robot_status_cb(carma_driver_msgs::msg::RobotEnabled::UniquePtr msg) + { + gsm_.onRoboticStatus(move(msg)); + } + + void GuidanceWorker::vehicle_status_cb(autoware_msgs::msg::VehicleStatus::UniquePtr msg) + { + gsm_.onVehicleStatus(move(msg)); + } + +} // guidance + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(guidance::GuidanceWorker) diff --git a/guidance/src/guidance/guidance_node.cpp b/guidance/src/main.cpp similarity index 62% rename from guidance/src/guidance/guidance_node.cpp rename to guidance/src/main.cpp index ca3aa2e6ab..2e45959cbf 100644 --- a/guidance/src/guidance/guidance_node.cpp +++ b/guidance/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * 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 @@ -14,15 +14,20 @@ * the License. */ -#include +#include #include "guidance/guidance_worker.hpp" -/*! - * \brief Main entry point for Guidance package - */ -int main(int argc, char** argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "guidance"); - guidance::GuidanceWorker worker; - return worker.run(); + 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/guidance/test/guidance_launch.test b/guidance/test/guidance_launch.test deleted file mode 100644 index 7570f0dff2..0000000000 --- a/guidance/test/guidance_launch.test +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - diff --git a/guidance/test/guidance_node_test.cpp b/guidance/test/guidance_node_test.cpp index e14d322fc1..8e5abd8e48 100644 --- a/guidance/test/guidance_node_test.cpp +++ b/guidance/test/guidance_node_test.cpp @@ -1,7 +1,7 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-2022 LEIDOS. * - * Licensed under the Apache License, Version 2.0 (the "License") { you may not + * 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 * @@ -15,34 +15,49 @@ */ #include -#include -#include -#include -#include - -TEST(GuidanceNodeTest, TestGuidanceLaunch) { - ros::NodeHandle nh = ros::NodeHandle(); - const cav_msgs::GuidanceState* state; - ros::Subscriber state_sub = nh.subscribe("state", 10, [&](cav_msgs::GuidanceStateConstPtr msg){ - state = msg.get(); - }); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - EXPECT_EQ(cav_msgs::GuidanceState::STARTUP, state->state); -} +#include "guidance/guidance_worker.hpp" +#include "guidance_test_suite.cpp" + +TEST(GuidanceNodeTest, TestGuidanceLaunch){ + // Activate the guidance node + rclcpp::NodeOptions options; + auto guidance_worker_node = std::make_shared(options); + guidance_worker_node->configure(); //Call configure state transition + guidance_worker_node->activate(); //Call activate state transition to get not read for runtime; + + // Activate the GuidanceTestSuite node + rclcpp::NodeOptions options2; + auto test_suite_node = std::make_shared(options2); + test_suite_node->configure(); + test_suite_node->activate(); + + // Add these nodes to an executor to spin them and trigger their callbacks + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(guidance_worker_node->get_node_base_interface()); + executor.add_node(test_suite_node->get_node_base_interface()); + + // Spin executor for 500 milliseconds + auto end_time = std::chrono::system_clock::now() + std::chrono::milliseconds(500); + while(std::chrono::system_clock::now() < end_time){ + executor.spin_once(); + } + + // Verify that the GuidnaceTestSuite node has received a DRIVERS_READY GuidanceState message since the Guidance node has been activated + EXPECT_EQ(carma_planning_msgs::msg::GuidanceState::DRIVERS_READY, test_suite_node->latest_state.state); +} -/*! - * \brief Main entrypoint for unit tests - */ -int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "guidance_test"); +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); - std::thread spinner([] {while (ros::ok()) ros::spin();}); + //Initialize ROS + rclcpp::init(argc, argv); - auto res = RUN_ALL_TESTS(); + bool success = RUN_ALL_TESTS(); - ros::shutdown(); + //shutdown ROS + rclcpp::shutdown(); - return res; -} + return success; +} \ No newline at end of file diff --git a/guidance/test/guidance_test_suite.cpp b/guidance/test/guidance_test_suite.cpp new file mode 100644 index 0000000000..c3bf63c2ad --- /dev/null +++ b/guidance/test/guidance_test_suite.cpp @@ -0,0 +1,61 @@ +/* + * 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. + */ + +#include +#include "guidance/guidance_worker.hpp" + +namespace guidance_test_suite +{ + + namespace std_ph = std::placeholders; + + /** + * GuidanceTestSuite: Test fixture for GuidanceWorker testing + * Maintains publishers, subscribers, and message tracking for all tests. + * State is reset between tests to ensure clean results. + */ + class GuidanceTestSuite : public carma_ros2_utils::CarmaLifecycleNode + { + + public: + /*! + * \brief Constructor for GuidanceTestSuite + */ + explicit GuidanceTestSuite(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) {} + + // Member for storing the latest Guidance State message + carma_planning_msgs::msg::GuidanceState latest_state; + + // Subscribers + carma_ros2_utils::SubPtr state_sub_; + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) { + // Setup Subscribers + state_sub_ = create_subscription("state", 5, + std::bind(&GuidanceTestSuite::stateCallback, this, std_ph::_1)); + + return CallbackReturn::SUCCESS; + } + + // Callback Function for Guidance State + void stateCallback(carma_planning_msgs::msg::GuidanceState::UniquePtr msg) { + latest_state = *msg; + } + + }; + +} // guidance_test_suite \ No newline at end of file diff --git a/guidance/test/test_guidance_state_machine.cpp b/guidance/test/test_guidance_state_machine.cpp index 764d7386bf..2c2561c57d 100644 --- a/guidance/test/test_guidance_state_machine.cpp +++ b/guidance/test/test_guidance_state_machine.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -20,84 +20,95 @@ TEST(GuidanceStateMachineTest, testStates) { - guidance::GuidanceStateMachine gsm; + auto node = std::make_shared("test_node"); + auto logger_interface = node->get_node_logging_interface(); + guidance::GuidanceStateMachine gsm(logger_interface); + // test initial state EXPECT_EQ(1, static_cast(gsm.getCurrentState())); + // test SetEnableRobotic call flag EXPECT_TRUE(!gsm.shouldCallSetEnableRobotic()); gsm.onSetGuidanceActive(true); - // test engage on startup state, should not casue state change + + // test engage on startup state, should not cause state change EXPECT_EQ(1, static_cast(gsm.getCurrentState())); - cav_msgs::SystemAlert alert; - alert.type = alert.DRIVERS_READY; - cav_msgs::SystemAlertConstPtr alert_pointer(new cav_msgs::SystemAlert(alert)); - gsm.onSystemAlert(alert_pointer); + // test drivers ready state + gsm.onGuidanceInitialized(); EXPECT_EQ(2, static_cast(gsm.getCurrentState())); gsm.onSetGuidanceActive(true); + // test active state EXPECT_EQ(3, static_cast(gsm.getCurrentState())); EXPECT_TRUE(gsm.shouldCallSetEnableRobotic()); EXPECT_TRUE(!gsm.shouldCallSetEnableRobotic()); gsm.onSetGuidanceActive(false); + // test disengage on active EXPECT_EQ(2, static_cast(gsm.getCurrentState())); gsm.onSetGuidanceActive(true); EXPECT_EQ(3, static_cast(gsm.getCurrentState())); - cav_msgs::RobotEnabled engage_status; + carma_driver_msgs::msg::RobotEnabled engage_status; engage_status.robot_enabled = true; engage_status.robot_active = true; - cav_msgs::RobotEnabled disengage_status; + carma_driver_msgs::msg::RobotEnabled disengage_status; disengage_status.robot_enabled = true; disengage_status.robot_active = false; - cav_msgs::RobotEnabledConstPtr engage_status_pointer(new cav_msgs::RobotEnabled(engage_status)); - cav_msgs::RobotEnabledConstPtr disengage_status_pointer(new cav_msgs::RobotEnabled(disengage_status)); - gsm.onRoboticStatus(disengage_status_pointer); + std::unique_ptr engage_status_pointer = std::make_unique(engage_status); + std::unique_ptr disengage_status_pointer = std::make_unique(disengage_status); + gsm.onRoboticStatus(move(disengage_status_pointer)); EXPECT_EQ(3, static_cast(gsm.getCurrentState())); - gsm.onRoboticStatus(engage_status_pointer); + gsm.onRoboticStatus(move(engage_status_pointer)); + // test engaged state EXPECT_EQ(4, static_cast(gsm.getCurrentState())); - gsm.onRoboticStatus(engage_status_pointer); + std::unique_ptr engage_status_pointer_2 = std::make_unique(engage_status); + gsm.onRoboticStatus(move(engage_status_pointer_2)); EXPECT_EQ(4, static_cast(gsm.getCurrentState())); gsm.onSetGuidanceActive(false); - gsm.onRoboticStatus(disengage_status_pointer); + std::unique_ptr disengage_status_pointer_2 = std::make_unique(disengage_status); + gsm.onRoboticStatus(move(disengage_status_pointer_2)); + // test disengage and restart state EXPECT_EQ(2, static_cast(gsm.getCurrentState())); gsm.onSetGuidanceActive(true); - gsm.onRoboticStatus(engage_status_pointer); + std::unique_ptr engage_status_pointer_3 = std::make_unique(engage_status); + gsm.onRoboticStatus(move(engage_status_pointer_3)); + // test re-engage state EXPECT_EQ(4, static_cast(gsm.getCurrentState())); - gsm.onRoboticStatus(disengage_status_pointer); + std::unique_ptr disengage_status_pointer_3 = std::make_unique(disengage_status); + gsm.onRoboticStatus(move(disengage_status_pointer_3)); + // test manual override and inactive state EXPECT_EQ(5, static_cast(gsm.getCurrentState())); gsm.onSetGuidanceActive(true); - gsm.onRoboticStatus(engage_status_pointer); + std::unique_ptr engage_status_pointer_4 = std::make_unique(engage_status); + gsm.onRoboticStatus(move(engage_status_pointer_4)); + // test re-engage state from inactive EXPECT_EQ(4, static_cast(gsm.getCurrentState())); - cav_msgs::RouteEvent route_event; - route_event.event = cav_msgs::RouteEvent::ROUTE_COMPLETED; - cav_msgs::RouteEventConstPtr route_event_pointer(new cav_msgs::RouteEvent(route_event)); - gsm.onRouteEvent(route_event_pointer); + carma_planning_msgs::msg::RouteEvent route_event; + route_event.event = carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED; + std::unique_ptr route_event_pointer = std::make_unique(route_event); + gsm.onRouteEvent(move(route_event_pointer)); + // test ENTER_PARK state at end of route EXPECT_EQ(6, static_cast(gsm.getCurrentState())); - autoware_msgs::VehicleStatus vehicle_status; - vehicle_status.current_gear.gear = autoware_msgs::Gear::PARK; // '3' indicates gearshift is set to Park - autoware_msgs::VehicleStatusConstPtr vehicle_status_pointer(new autoware_msgs::VehicleStatus(vehicle_status)); - gsm.onVehicleStatus(vehicle_status_pointer); + autoware_msgs::msg::VehicleStatus vehicle_status; + vehicle_status.current_gear.gear = autoware_msgs::msg::Gear::PARK; // '3' indicates gearshift is set to Park + std::unique_ptr vehicle_status_pointer = std::make_unique(vehicle_status); + gsm.onVehicleStatus(move(vehicle_status_pointer)); + // test DRIVERS_READY state EXPECT_EQ(2, static_cast(gsm.getCurrentState())); - alert.type = alert.SHUTDOWN; - cav_msgs::SystemAlertConstPtr alert_pointer_2(new cav_msgs::SystemAlert(alert)); - gsm.onSystemAlert(alert_pointer_2); + // test shut down state + gsm.onGuidanceShutdown(); EXPECT_EQ(0, static_cast(gsm.getCurrentState())); + // Should not recover from OFF state - gsm.onSystemAlert(alert_pointer); + gsm.onGuidanceInitialized(); EXPECT_EQ(0, static_cast(gsm.getCurrentState())); -} - -// Run all the tests -int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +} \ No newline at end of file From b5daa9616a54f9aec8b0f22214f121fe7dee55a2 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 30 Jun 2022 13:10:28 -0400 Subject: [PATCH 027/165] Feature/arbitrator (#1833) Description Porting arbitrator node to ROS2. Major changes include: Created ArbitratorNode class to fit the carma_lifecycle_node constructor API. And kept arbitrator worker class concept where it can accept interfaces. Previously Arbitrator worker class was directly created at main.cpp and accepted interface classes as dependency injections by passing by reference. This worked because main.cpp never went out of scope and references worked. However, in CarmaLifecycle paradigm, once the handle_on_configure finished, those references go out of scope and undefined behaviors arise. Therefore all reference instances changed to shared_ptr. Previously Arbitrator worker class called run() function and ran a while loop perpetually using sleep function to execute next iteration. Since no function can run indefinitely in CarmaLifecycle paradigm (or I didn't find a way), this run() function was ported as timercallback function with the planning frequency paramter (1hz). Related Issue #1832 --- .gitignore | 1 + arbitrator/CMakeLists.txt | 144 ++++--------- arbitrator/config/arbitrator_params.yaml | 15 +- arbitrator/include/arbitrator.hpp | 78 ++++--- arbitrator/include/arbitrator_config.hpp | 76 +++++++ arbitrator/include/arbitrator_node.hpp | 76 +++++++ .../include/arbitrator_state_machine.hpp | 4 +- arbitrator/include/arbitrator_utils.hpp | 42 ++-- arbitrator/include/beam_search_strategy.hpp | 4 +- arbitrator/include/capabilities_interface.hpp | 33 +-- arbitrator/include/capabilities_interface.tpp | 25 ++- arbitrator/include/cost_function.hpp | 10 +- .../include/cost_system_cost_function.hpp | 16 +- .../include/fixed_priority_cost_function.hpp | 8 +- arbitrator/include/neighbor_generator.hpp | 6 +- arbitrator/include/planning_strategy.hpp | 6 +- .../include/plugin_neighbor_generator.hpp | 4 +- .../include/plugin_neighbor_generator.tpp | 40 ++-- arbitrator/include/search_strategy.hpp | 6 +- arbitrator/include/tree_planner.hpp | 29 +-- arbitrator/include/vehicle_state.hpp | 5 +- arbitrator/launch/arbitrator.launch | 21 -- arbitrator/launch/arbitrator.launch.py | 68 +++++++ arbitrator/package.xml | 41 +++- arbitrator/src/arbitrator.cpp | 105 +++++----- arbitrator/src/arbitrator_node.cpp | 192 +++++++++++------- arbitrator/src/arbitrator_state_machine.cpp | 10 +- arbitrator/src/arbitrator_utils.cpp | 28 +-- arbitrator/src/beam_search_strategy.cpp | 6 +- arbitrator/src/capabilities_interface.cpp | 22 +- arbitrator/src/cost_system_cost_function.cpp | 28 +-- .../src/fixed_priority_cost_function.cpp | 12 +- arbitrator/src/main.cpp | 35 ++++ arbitrator/src/tree_planner.cpp | 52 ++--- .../test/test_arbitrator_state_machine.cpp | 2 +- arbitrator/test/test_beam_search_strategy.cpp | 100 ++++----- .../test_fixed_priority_cost_function.cpp | 62 +++--- arbitrator/test/test_main.cpp | 2 +- .../test/test_plugin_neighbor_generator.cpp | 55 ++--- arbitrator/test/test_tree_planner.cpp | 147 +++++++------- arbitrator/test/test_utils.h | 3 +- carma/launch/guidance.launch | 3 - carma/launch/guidance.launch.py | 20 ++ 43 files changed, 975 insertions(+), 667 deletions(-) create mode 100644 arbitrator/include/arbitrator_config.hpp create mode 100644 arbitrator/include/arbitrator_node.hpp delete mode 100644 arbitrator/launch/arbitrator.launch create mode 100644 arbitrator/launch/arbitrator.launch.py create mode 100644 arbitrator/src/main.cpp diff --git a/.gitignore b/.gitignore index 3456d8b050..cf9b0c9a8b 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,7 @@ myClock.bin /*.tar.gz *.class **/bin +**/__pycache__ # ignore build directories build diff --git a/arbitrator/CMakeLists.txt b/arbitrator/CMakeLists.txt index e2b63e2272..b61667fde6 100644 --- a/arbitrator/CMakeLists.txt +++ b/arbitrator/CMakeLists.txt @@ -1,5 +1,5 @@ -# Copyright (C) 2019-2021 LEIDOS. +# 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 @@ -13,136 +13,76 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(arbitrator) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - carma_utils - cav_msgs - cav_srvs - roscpp - lanelet2_core - carma_wm - tf - tf2 - tf2_geometry_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES arbitrator - CATKIN_DEPENDS carma_utils cav_msgs cav_srvs roscpp lanelet2_core carma_wm tf tf2 tf2_geometry_msgs -# DEPENDS system_lib -) +# Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -########### -## Build ## -########### +# Name build targets +set(node_exec arbitrator_node_exec) +set(node_lib arbitrator_node_lib) -## Specify additional locations of header files -## Your package locations should be listed before other locations +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} ) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node - src/arbitrator_node.cpp) - -## Add cmake target dependencies of the executable -add_library(arbitrator_library +# Build +ament_auto_add_library(${node_lib} SHARED src/arbitrator_state_machine.cpp src/arbitrator_utils.cpp src/arbitrator.cpp + src/arbitrator_node.cpp src/beam_search_strategy.cpp src/capabilities_interface.cpp src/fixed_priority_cost_function.cpp src/cost_system_cost_function.cpp - src/tree_planner.cpp) - - -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(arbitrator_library ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against - -target_link_libraries(arbitrator_library ${catkin_LIBRARIES}) - -target_link_libraries(${PROJECT_NAME}_node - arbitrator_library + src/tree_planner.cpp ) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS ${PROJECT_NAME}_node arbitrator_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +ament_auto_add_executable(${node_exec} + src/main.cpp ) -# Mark cpp header files for installation -install(DIRECTORY include/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.hpp" - PATTERN ".svn" EXCLUDE -) -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - config - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Register component +rclcpp_components_register_nodes(${node_lib} "arbitrator::ArbitratorNode") + +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -############# -## Testing ## -############# +# Testing + if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -catkin_add_gmock(${PROJECT_NAME}-test + ament_add_gmock(test_arbitrator test/test_arbitrator_state_machine.cpp test/test_plugin_neighbor_generator.cpp test/test_fixed_priority_cost_function.cpp test/test_beam_search_strategy.cpp test/test_tree_planner.cpp - test/test_main.cpp) + test/test_main.cpp +) + + ament_target_dependencies(test_arbitrator ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test arbitrator_library ${catkin_LIBRARIES}) -endif() + target_link_libraries(test_arbitrator ${node_lib}) + + endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/arbitrator/config/arbitrator_params.yaml b/arbitrator/config/arbitrator_params.yaml index c14f6834a6..2590564a26 100644 --- a/arbitrator/config/arbitrator_params.yaml +++ b/arbitrator/config/arbitrator_params.yaml @@ -23,7 +23,20 @@ beam_width: 3 # Unit: N/a use_fixed_costs: true + # JSON String: Plugin Priorities Map +# It would have been preferable to use a dictionary here but ROS2 Foxy does not support this type of parameter # Map: The priorities associated with each plugin during the planning # process, values will be normalized at runtime and inverted into costs # Unit: N/a -plugin_priorities: {PlatooningStrategicIHPPlugin: 4.0, LCIStrategicPlugin: 3.0, SCIStrategicPlugin: 2.0, PlatooningStrategicPlugin: 2.0, RouteFollowingPlugin: 1.0} \ No newline at end of file +plugin_priorities: ' + { + "plugin_priorities": + { + "PlatooningStrategicIHPPlugin": 4.0, + "LCIStrategicPlugin": 3.0, + "SCIStrategicPlugin": 2.0, + "PlatooningStrategicPlugin": 2.0, + "RouteFollowingPlugin": 1.0 + } + } + ' diff --git a/arbitrator/include/arbitrator.hpp b/arbitrator/include/arbitrator.hpp index 9120d2acc9..ec373a3f08 100644 --- a/arbitrator/include/arbitrator.hpp +++ b/arbitrator/include/arbitrator.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -17,20 +17,21 @@ #ifndef __ARBITRATOR_INCLUDE_ARBITRATOR_HPP__ #define __ARBITRATOR_INCLUDE_ARBITRATOR_HPP__ -#include -#include -#include "arbitrator_state_machine.hpp" -#include "planning_strategy.hpp" -#include "capabilities_interface.hpp" -#include -#include "vehicle_state.hpp" -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include +#include "vehicle_state.hpp" +#include "arbitrator_state_machine.hpp" +#include "planning_strategy.hpp" +#include "capabilities_interface.hpp" + namespace arbitrator { /** @@ -40,13 +41,12 @@ namespace arbitrator * the CARMA planning process. Utilizes a generic planning interface to allow * for reconfiguration with other paradigms in the future. */ - class Arbitrator + class Arbitrator { public: /** * \brief Constructor for arbitrator class taking in dependencies via dependency injection - * \param nh A CARMANodeHandle instance with a globally referenced ("/") path - * \param pnh A CARMANodeHandle instance with a privately referenced ("~") path + * \param nh A CarmaLifecycleNode node pointer * \param sm An ArbitratorStateMachine instance for regulating the states of the Arbitrator * \param ci A CapabilitiesInterface for querying plugins * \param planning_strategy A planning strategy implementation for generating plans @@ -54,28 +54,27 @@ namespace arbitrator * \param planning_frequency The frequency at which to generate high-level plans when engaged * \param wm pointer to an inialized world model. */ - Arbitrator(ros::CARMANodeHandle *nh, - ros::CARMANodeHandle *pnh, - ArbitratorStateMachine *sm, - CapabilitiesInterface *ci, - PlanningStrategy &planning_strategy, - ros::Duration min_plan_duration, - ros::Rate planning_frequency, - carma_wm::WorldModelConstPtr wm): + Arbitrator(std::shared_ptr nh, + 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): sm_(sm), nh_(nh), - pnh_(pnh), capabilities_interface_(ci), planning_strategy_(planning_strategy), initialized_(false), min_plan_duration_(min_plan_duration), - time_between_plans_(planning_frequency.expectedCycleTime()), - wm_(wm) {}; + time_between_plans_(planning_period), + wm_(wm), + tf2_buffer_(nh_->get_clock()) {}; /** * \brief Begin the operation of the arbitrator. * - * Loops internally via ros::Duration sleeps and spins + * Loops internally via rclcpp::Duration sleeps and spins */ void run(); @@ -83,7 +82,7 @@ namespace arbitrator * \brief Callback for the twist subscriber, which will store latest twist locally * \param msg Latest twist message */ - void twist_cb(const geometry_msgs::TwistStampedConstPtr& msg); + void twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg); /** * \brief Callback for the front bumper pose transform @@ -127,32 +126,31 @@ namespace arbitrator * \brief Callback for receiving Guidance state machine updates * \param msg The new GuidanceState message */ - void guidance_state_cb(const cav_msgs::GuidanceState::ConstPtr& msg); + 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 - ArbitratorStateMachine *sm_; - ros::Publisher final_plan_pub_; - ros::Subscriber guidance_state_sub_; - ros::CARMANodeHandle *nh_; - ros::CARMANodeHandle *pnh_; - ros::Duration min_plan_duration_; - ros::Duration time_between_plans_; - ros::Time next_planning_process_start_; - CapabilitiesInterface *capabilities_interface_; - PlanningStrategy &planning_strategy_; + std::shared_ptr sm_; + carma_ros2_utils::PubPtr final_plan_pub_; + carma_ros2_utils::SubPtr guidance_state_sub_; + std::shared_ptr nh_; + rclcpp::Duration min_plan_duration_; + rclcpp::Duration time_between_plans_; + rclcpp::Time next_planning_process_start_; + std::shared_ptr capabilities_interface_; + std::shared_ptr planning_strategy_; bool initialized_; carma_wm::WorldModelConstPtr wm_; - geometry_msgs::TransformStamped tf_; + geometry_msgs::msg::TransformStamped tf_; // TF listenser tf2_ros::Buffer tf2_buffer_; std::unique_ptr tf2_listener_; // transform from front bumper to map tf2::Stamped bumper_transform_; - + bool planning_in_progress_ = false; }; }; diff --git a/arbitrator/include/arbitrator_config.hpp b/arbitrator/include/arbitrator_config.hpp new file mode 100644 index 0000000000..9562104a9d --- /dev/null +++ b/arbitrator/include/arbitrator_config.hpp @@ -0,0 +1,76 @@ +/* + * 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 __ARBITRATOR_INCLUDE_ARBITRATOR_CONFIG_HPP__ +#define __ARBITRATOR_INCLUDE_ARBITRATOR_CONFIG_HPP__ + +#include +#include + +#include +#include +#include +#include + + +namespace arbitrator +{ + // Stream operator for map data structure + std::ostream &operator<<(std::ostream &output, const std::map &map) + { + output << "Map { " << std::endl; + for (auto const& pair : map) + { + output << pair.first << ": " << pair.second << std::endl; + } + 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 + // 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 + // 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 = + // 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 + // evaluating maneuver plans + 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) + { + output << "Arbitrator::Config { " << std::endl + << "min_plan_duration: " << c.min_plan_duration << std::endl + << "target_plan_duration: " << c.target_plan_duration << std::endl + << "planning_frequency: " << c.planning_frequency << std::endl + << "beam_width: " << c.beam_width << std::endl + << "use_fixed_costs: " << c.use_fixed_costs << std::endl + << "plugin_priorities: " << c.plugin_priorities << std::endl + << "}" << std::endl; + return output; + }; + }; + +} // namespace abitrator + +#endif \ No newline at end of file diff --git a/arbitrator/include/arbitrator_node.hpp b/arbitrator/include/arbitrator_node.hpp new file mode 100644 index 0000000000..81c551ed7b --- /dev/null +++ b/arbitrator/include/arbitrator_node.hpp @@ -0,0 +1,76 @@ +/* + * 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 __ARBITRATOR_INCLUDE_ARBITRATOR_NODE_HPP__ +#define __ARBITRATOR_INCLUDE_ARBITRATOR_NODE_HPP__ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "arbitrator.hpp" +#include "arbitrator_config.hpp" +#include "arbitrator_state_machine.hpp" +#include "cost_system_cost_function.hpp" +#include "fixed_priority_cost_function.hpp" +#include "plugin_neighbor_generator.hpp" +#include "beam_search_strategy.hpp" +#include "tree_planner.hpp" + + +namespace arbitrator +{ + /** + * An arbitrator node instance with currently used configuration/planning paradigms + * + * Governs the interactions of plugins during the maneuver planning phase of + * the CARMA planning process by internally using a worker class with generic planning interface. + * + */ + class ArbitratorNode : public carma_ros2_utils::CarmaLifecycleNode + { + public: + /** + * @brief Constructor + */ + explicit ArbitratorNode(const rclcpp::NodeOptions& options); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + + private: + // helper function to parse plugin_priorities param from yaml as a json + std::map plugin_priorities_map_from_json(const std::string& json_string); + + Config config_; + std::shared_ptr arbitrator_; + rclcpp::TimerBase::SharedPtr bumper_pose_timer_; + rclcpp::TimerBase::SharedPtr arbitrator_run_; + + }; + +} // namespace abitrator + +#endif \ No newline at end of file diff --git a/arbitrator/include/arbitrator_state_machine.hpp b/arbitrator/include/arbitrator_state_machine.hpp index 1f540698f0..d48b5f2a58 100644 --- a/arbitrator/include/arbitrator_state_machine.hpp +++ b/arbitrator/include/arbitrator_state_machine.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -99,7 +99,7 @@ namespace arbitrator {INITIAL, SYSTEM_STARTUP_COMPLETE, PLANNING}, {PLANNING, PLANNING_COMPLETE, WAITING}, {WAITING, PLANNING_TIMER_TRIGGER, PLANNING}, - + // Interrupt and resume {PLANNING, ARBITRATOR_PAUSED, PAUSED}, {WAITING, ARBITRATOR_PAUSED, PAUSED}, diff --git a/arbitrator/include/arbitrator_utils.hpp b/arbitrator/include/arbitrator_utils.hpp index 7af546d38d..1c00c68d8b 100644 --- a/arbitrator/include/arbitrator_utils.hpp +++ b/arbitrator/include/arbitrator_utils.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -17,8 +17,8 @@ #ifndef __ARBITRATOR_INCLUDE_ARBITRATOR_UTILS_HPP__ #define __ARBITRATOR_INCLUDE_ARBITRATOR_UTILS_HPP__ -#include -#include +#include +#include /** * \brief Macro definition to enable easier access to fields shared across the maneuver typees @@ -31,12 +31,12 @@ * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field */ #define GET_MANEUVER_PROPERTY(mvr, property)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\ throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id")))))))) namespace arbitrator_utils @@ -44,10 +44,10 @@ namespace arbitrator_utils /** * \brief Get the start time of the first maneuver in the plan * \param plan The plan to examine - * \return The ros::Time at which it starts + * \return The rclcpp::Time at which it starts * \throws An invalid argument exception if the plan is empty */ - ros::Time get_plan_start_time(const cav_msgs::ManeuverPlan&); + rclcpp::Time get_plan_start_time(const carma_planning_msgs::msg::ManeuverPlan&); /** * \brief Get the start distance of the first maneuver in the plan @@ -55,15 +55,15 @@ namespace arbitrator_utils * \return The double-valued linear downtrack distance in meters to the start of the plan * \throws An invalid argument exception if the plan is empty */ - double get_plan_start_distance(const cav_msgs::ManeuverPlan&); + double get_plan_start_distance(const carma_planning_msgs::msg::ManeuverPlan&); /** * \brief Get the end time of the first maneuver in the plan * \param plan The plan to examine - * \return The ros::Time at which it ends + * \return The rclcpp::Time at which it ends * \throws An invalid argument exception if the plan is empty */ - ros::Time get_plan_end_time(const cav_msgs::ManeuverPlan&); + rclcpp::Time get_plan_end_time(const carma_planning_msgs::msg::ManeuverPlan&); /** * \brief Get the end distance of the first maneuver in the plan @@ -71,15 +71,15 @@ namespace arbitrator_utils * \return The double-valued linear downtrack distance in meters to the end of the plan * \throws An invalid argument exception if the plan is empty */ - double get_plan_end_distance(const cav_msgs::ManeuverPlan&); + double get_plan_end_distance(const carma_planning_msgs::msg::ManeuverPlan&); /** * \brief Get the start time of the specified maneuver * \param mvr The maneuver to examine - * \return The ros::Time at which it starts + * \return The rclcpp::Time at which it starts * \throws An invalid argument exception if the maneuver is poorly constructed */ - ros::Time get_maneuver_start_time(const cav_msgs::Maneuver&); + rclcpp::Time get_maneuver_start_time(const carma_planning_msgs::msg::Maneuver&); /** * \brief Get the start distance the specified maneuver @@ -87,15 +87,15 @@ namespace arbitrator_utils * \return The double-valued linear downtrack distance in meters to the start of the maneuver * \throws An invalid argument exception if the maneuver is poorly constructed */ - double get_maneuver_start_distance(const cav_msgs::Maneuver&); + double get_maneuver_start_distance(const carma_planning_msgs::msg::Maneuver&); /** * \brief Get the end time of the specified maneuver * \param mvr The maneuver to examine - * \return The ros::Time at which it ends + * \return The rclcpp::Time at which it ends * \throws An invalid argument exception if the maneuver is poorly constructed */ - ros::Time get_maneuver_end_time(const cav_msgs::Maneuver&); + rclcpp::Time get_maneuver_end_time(const carma_planning_msgs::msg::Maneuver&); /** * \brief Get the end distance of the specified maneuver @@ -103,7 +103,7 @@ namespace arbitrator_utils * \return The double-valued linear downtrack distance in meters to the end of the maneuver * \throws An invalid argument exception if the maneuver is poorly constructed */ - double get_maneuver_end_distance(const cav_msgs::Maneuver&); + double get_maneuver_end_distance(const carma_planning_msgs::msg::Maneuver&); } // namespace arbitrator #endif //__ARBITRATOR_INCLUDE_ARBITRATOR_UTILS_HPP__ diff --git a/arbitrator/include/beam_search_strategy.hpp b/arbitrator/include/beam_search_strategy.hpp index 73caf0bde0..4f479c0040 100644 --- a/arbitrator/include/beam_search_strategy.hpp +++ b/arbitrator/include/beam_search_strategy.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -51,7 +51,7 @@ namespace arbitrator * \param plans The plans to evaluate as (plan, cot ) pairs * \return The sorted list of up to size beam_width */ - std::vector> prioritize_plans(std::vector> plans) const; + std::vector> prioritize_plans(std::vector> plans) const; private: int beam_width_; }; diff --git a/arbitrator/include/capabilities_interface.hpp b/arbitrator/include/capabilities_interface.hpp index 8d7829be15..4bca7f7bca 100644 --- a/arbitrator/include/capabilities_interface.hpp +++ b/arbitrator/include/capabilities_interface.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -17,14 +17,15 @@ #ifndef __ARBITRATOR_INCLUDE_CAPABILITIES_INTERFACE_HPP__ #define __ARBITRATOR_INCLUDE_CAPABILITIES_INTERFACE_HPP__ -#include -#include +#include +#include #include #include #include #include -#include -#include +#include +#include + namespace arbitrator { @@ -37,10 +38,10 @@ namespace arbitrator public: /** * \brief Constructor for Capabilities interface - * \param nh A publically addressesed ("/") ros::NodeHandle + * \param nh A CarmaLifecycleNode pointer this interface will work with */ - CapabilitiesInterface(ros::NodeHandle *nh): nh_(nh) { - sc_s = nh_->serviceClient("plugins/get_strategic_plugin_by_capability"); + CapabilitiesInterface(std::shared_ptr nh): nh_(nh) { + sc_s_ = nh_->create_client("plugins/get_strategic_plugin_by_capability"); }; /** @@ -61,28 +62,32 @@ namespace arbitrator /** * \brief Template function for calling all nodes which respond to a service associated - * with a particular capabilitiy. 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 MSrv The typename of the service message + * \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(std::string query_string, MSrv msg); + 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: private: - ros::NodeHandle *nh_; + std::shared_ptr nh_; - ros::ServiceClient sc_s; + carma_ros2_utils::ClientPtr sc_s_; std::unordered_set capabilities_ ; }; + }; #include "capabilities_interface.tpp" diff --git a/arbitrator/include/capabilities_interface.tpp b/arbitrator/include/capabilities_interface.tpp index 7046e24ae0..e7734c6a26 100644 --- a/arbitrator/include/capabilities_interface.tpp +++ b/arbitrator/include/capabilities_interface.tpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * Copyright (C) 2022 LEIDOS. * * Licensed under the Apache License, Version 2.0 (the "License"); you may not * use this file except in compliance with the License. You may obtain a copy of @@ -21,21 +21,28 @@ #include #include #include -#include +#include namespace arbitrator { - template - std::map CapabilitiesInterface::multiplex_service_call_for_capability(std::string query_string, MSrv msg) + template + 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; + std::map> responses; + for (auto i = topics.begin(); i != topics.end(); i++) { - ros::ServiceClient sc = nh_->serviceClient(*i); - ROS_DEBUG_STREAM("found client: " << *i); - if (sc.call(msg)) { - responses.emplace(*i, msg); + auto sc = nh_->create_client(*i); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "found client: " << *i); + + 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(*i, resp.get()); } } return responses; diff --git a/arbitrator/include/cost_function.hpp b/arbitrator/include/cost_function.hpp index 95eca8781a..1cc9125a37 100644 --- a/arbitrator/include/cost_function.hpp +++ b/arbitrator/include/cost_function.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -17,8 +17,8 @@ #ifndef __ARBITRATOR_INCLUDE_COST_FUNCTION_HPP__ #define __ARBITRATOR_INCLUDE_COST_FUNCTION_HPP__ -#include -#include +#include +#include namespace arbitrator { @@ -34,14 +34,14 @@ namespace arbitrator * \param plan The plan to evaluate * \return double The total cost */ - virtual double compute_total_cost(const cav_msgs::ManeuverPlan& plan) = 0; + virtual double compute_total_cost(const carma_planning_msgs::msg::ManeuverPlan& plan) = 0; /** * \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 */ - virtual double compute_cost_per_unit_distance(const cav_msgs::ManeuverPlan& plan) = 0; + virtual double compute_cost_per_unit_distance(const carma_planning_msgs::msg::ManeuverPlan& plan) = 0; /** * \brief Virtual destructor provided for memory safety diff --git a/arbitrator/include/cost_system_cost_function.hpp b/arbitrator/include/cost_system_cost_function.hpp index db0bf7e196..affb888cdf 100644 --- a/arbitrator/include/cost_system_cost_function.hpp +++ b/arbitrator/include/cost_system_cost_function.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -17,10 +17,12 @@ #ifndef __ARBITRATOR_INCLUDE_COST_SYSTEM_COST_FUNCTION_HPP__ #define __ARBITRATOR_INCLUDE_COST_SYSTEM_COST_FUNCTION_HPP__ -#include -#include "cost_function.hpp" +#include #include #include +#include + +#include "cost_function.hpp" namespace arbitrator { @@ -47,7 +49,7 @@ namespace arbitrator * * \param nh A publicly namespaced nodehandle */ - void init(ros::NodeHandle &nh); + void init(std::shared_ptr nh); /** * \brief Compute the unit cost over distance of a given maneuver plan @@ -56,7 +58,7 @@ namespace arbitrator * * \throws std::logic_error if not initialized */ - double compute_total_cost(const cav_msgs::ManeuverPlan& plan); + double compute_total_cost(const carma_planning_msgs::msg::ManeuverPlan& plan); /** * \brief Compute the unit cost over distance of a given maneuver plan @@ -64,9 +66,9 @@ namespace arbitrator * \return double The total cost divided by the total distance of the plan * \throws std::logic_error if not initialized */ - double compute_cost_per_unit_distance(const cav_msgs::ManeuverPlan& plan); + double compute_cost_per_unit_distance(const carma_planning_msgs::msg::ManeuverPlan& plan); private: - ros::ServiceClient cost_system_sc_; + carma_ros2_utils::ClientPtr cost_system_sc_; bool initialized_ = false; }; }; diff --git a/arbitrator/include/fixed_priority_cost_function.hpp b/arbitrator/include/fixed_priority_cost_function.hpp index dec323e5e2..0e27d7d65a 100644 --- a/arbitrator/include/fixed_priority_cost_function.hpp +++ b/arbitrator/include/fixed_priority_cost_function.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -39,7 +39,7 @@ namespace arbitrator public: /** * \brief Constructor for FixedPriorityCostFunction - * \param nh A publically namespaced ("/") ros::NodeHandle + * \param plugin_priorities with plugin_name, priority_value mappings */ FixedPriorityCostFunction(const std::map &plugin_priorities); @@ -48,14 +48,14 @@ namespace arbitrator * \param plan The plan to evaluate * \return double The total cost divided by the total distance of the plan */ - double compute_total_cost(const cav_msgs::ManeuverPlan& plan); + double compute_total_cost(const carma_planning_msgs::msg::ManeuverPlan& plan); /** * \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 */ - double compute_cost_per_unit_distance(const cav_msgs::ManeuverPlan& plan); + double compute_cost_per_unit_distance(const carma_planning_msgs::msg::ManeuverPlan& plan); private: std::map plugin_costs_; }; diff --git a/arbitrator/include/neighbor_generator.hpp b/arbitrator/include/neighbor_generator.hpp index 3e63b25c8a..e90a73afe6 100644 --- a/arbitrator/include/neighbor_generator.hpp +++ b/arbitrator/include/neighbor_generator.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -18,7 +18,7 @@ #define __ARBITRATOR_INCLUDE_NEIGHBOR_GENERATOR_HPP__ #include -#include +#include #include "vehicle_state.hpp" namespace arbitrator @@ -38,7 +38,7 @@ namespace arbitrator * * \return A vector containing the new plans generated from it, if any */ - virtual std::vector generate_neighbors(cav_msgs::ManeuverPlan plan, const VehicleState& initial_state) const = 0; + virtual std::vector generate_neighbors(carma_planning_msgs::msg::ManeuverPlan plan, const VehicleState& initial_state) const = 0; /** * \brief Virtual destructor provided for memory safety diff --git a/arbitrator/include/planning_strategy.hpp b/arbitrator/include/planning_strategy.hpp index d4927f461f..c5f1dea529 100644 --- a/arbitrator/include/planning_strategy.hpp +++ b/arbitrator/include/planning_strategy.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -17,7 +17,7 @@ #ifndef __ARBITRATOR_INCLUDE_PLANNING_STRATEGY_HPP__ #define __ARBITRATOR_INCLUDE_PLANNING_STRATEGY_HPP__ -#include +#include #include "vehicle_state.hpp" namespace arbitrator @@ -37,7 +37,7 @@ namespace arbitrator * * \return A maneuver plan from the vehicle's current state */ - virtual cav_msgs::ManeuverPlan generate_plan(const VehicleState& start_state) = 0; + virtual carma_planning_msgs::msg::ManeuverPlan generate_plan(const VehicleState& start_state) = 0; /** * \brief Virtual destructor provided for memory safety diff --git a/arbitrator/include/plugin_neighbor_generator.hpp b/arbitrator/include/plugin_neighbor_generator.hpp index 041caf2885..2717cc0e7f 100644 --- a/arbitrator/include/plugin_neighbor_generator.hpp +++ b/arbitrator/include/plugin_neighbor_generator.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -50,7 +50,7 @@ namespace arbitrator * \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 * \return A list of subsequent plans building on top of the input plan */ - std::vector generate_neighbors(cav_msgs::ManeuverPlan plan, const VehicleState& initial_state) const; + std::vector generate_neighbors(carma_planning_msgs::msg::ManeuverPlan plan, const VehicleState& initial_state) const; private: T &ci_; }; diff --git a/arbitrator/include/plugin_neighbor_generator.tpp b/arbitrator/include/plugin_neighbor_generator.tpp index 72cf34620a..ba27b28d68 100644 --- a/arbitrator/include/plugin_neighbor_generator.tpp +++ b/arbitrator/include/plugin_neighbor_generator.tpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -19,35 +19,37 @@ #include "capabilities_interface.hpp" #include "plugin_neighbor_generator.hpp" -#include +#include #include namespace arbitrator -{ +{ + using PlanMvrRes = carma_planning_msgs::srv::PlanManeuvers::Response; + using PlanMvrReq = carma_planning_msgs::srv::PlanManeuvers::Request; + template - std::vector PluginNeighborGenerator::generate_neighbors(cav_msgs::ManeuverPlan plan, const VehicleState& initial_state) const + std::vector PluginNeighborGenerator::generate_neighbors(carma_planning_msgs::msg::ManeuverPlan plan, const VehicleState& initial_state) const { - cav_srvs::PlanManeuvers msg; + auto msg = std::make_shared(); // Set prior plan - msg.request.prior_plan = plan; + msg->prior_plan = plan; // Set vehicle state at prior plan start - msg.request.header.frame_id = "map"; - msg.request.header.stamp = initial_state.stamp; - msg.request.veh_x = initial_state.x; - msg.request.veh_y = initial_state.y; - msg.request.veh_downtrack = initial_state.downtrack; - msg.request.veh_logitudinal_velocity = initial_state.velocity; - msg.request.veh_lane_id = std::to_string(initial_state.lane_id); - - std::map res = ci_.multiplex_service_call_for_capability(CapabilitiesInterface::STRATEGIC_PLAN_CAPABILITY, msg); - + msg->header.frame_id = "map"; + msg->header.stamp = initial_state.stamp; + msg->veh_x = initial_state.x; + msg->veh_y = initial_state.y; + msg->veh_downtrack = initial_state.downtrack; + msg->veh_logitudinal_velocity = initial_state.velocity; + msg->veh_lane_id = std::to_string(initial_state.lane_id); + + std::map> res = ci_.template multiplex_service_call_for_capability(CapabilitiesInterface::STRATEGIC_PLAN_CAPABILITY, msg); // Convert map to vector of map values - std::vector out; + std::vector out; for (auto it = res.begin(); it != res.end(); it++) { - ROS_DEBUG_STREAM("Pushing response of child: " << it->first << ", which had mvr size: " << it->second.response.new_plan.maneuvers.size()); - out.push_back(it->second.response.new_plan); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "Pushing response of child: " << it->first << ", which had mvr size: " << it->second->new_plan.maneuvers.size()); + out.push_back(it->second->new_plan); } return out; } diff --git a/arbitrator/include/search_strategy.hpp b/arbitrator/include/search_strategy.hpp index 2f569e516a..76f63ded02 100644 --- a/arbitrator/include/search_strategy.hpp +++ b/arbitrator/include/search_strategy.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -18,7 +18,7 @@ #define __ARBITRATOR_INCLUDE_SEARCH_STRATEGY_HPP__ #include -#include +#include namespace arbitrator { @@ -42,7 +42,7 @@ namespace arbitrator * \return A sorted (and/or reduced) list of (plan, cost) pairs after a * heuristic may or may not have been applied */ - virtual std::vector> prioritize_plans(std::vector> plans) const = 0; + virtual std::vector> prioritize_plans(std::vector> plans) const = 0; /** * \brief Virtual destructor provided for memory safety diff --git a/arbitrator/include/tree_planner.hpp b/arbitrator/include/tree_planner.hpp index 1725053ffd..80759b893c 100644 --- a/arbitrator/include/tree_planner.hpp +++ b/arbitrator/include/tree_planner.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -18,12 +18,13 @@ #define __ARBITRATOR_INCLUDE_TREE_PLANNER_HPP__ #include -#include +#include #include "planning_strategy.hpp" #include "cost_function.hpp" #include "neighbor_generator.hpp" #include "search_strategy.hpp" #include "vehicle_state.hpp" +#include namespace arbitrator { @@ -41,15 +42,15 @@ namespace arbitrator public: /** * \brief Tree planner constructor - * \param cf A reference to a CostFunction implementation - * \param ng A reference to a NeighborGenerator implementation - * \param ss A reference to a SearchStrategy implementation + * \param cf Shared ptr to a CostFunction implementation + * \param ng Shared ptr to a NeighborGenerator implementation + * \param ss Shared ptr to a SearchStrategy implementation * \param target The desired duration of finished plans */ - TreePlanner(CostFunction &cf, - NeighborGenerator &ng, - SearchStrategy &ss, - ros::Duration target): + TreePlanner(std::shared_ptr cf, + std::shared_ptr ng, + std::shared_ptr ss, + rclcpp::Duration target): cost_function_(cf), neighbor_generator_(ng), search_strategy_(ss), @@ -61,12 +62,12 @@ namespace arbitrator * * \param start_state The starting state of the vehicle to plan for */ - cav_msgs::ManeuverPlan generate_plan(const VehicleState& start_state); + carma_planning_msgs::msg::ManeuverPlan generate_plan(const VehicleState& start_state); protected: - CostFunction &cost_function_; - NeighborGenerator &neighbor_generator_; - SearchStrategy &search_strategy_; - ros::Duration target_plan_duration_; + std::shared_ptr cost_function_; + std::shared_ptr neighbor_generator_; + std::shared_ptr search_strategy_; + rclcpp::Duration target_plan_duration_; }; }; diff --git a/arbitrator/include/vehicle_state.hpp b/arbitrator/include/vehicle_state.hpp index 0432814217..9faec577f8 100644 --- a/arbitrator/include/vehicle_state.hpp +++ b/arbitrator/include/vehicle_state.hpp @@ -2,7 +2,7 @@ #define __ARBITRATOR_INCLUDE_VEHICLE_STATE_HPP__ /* - * Copyright (C) 2021 LEIDOS. + * 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 @@ -18,6 +18,7 @@ */ #include +#include namespace arbitrator { @@ -26,7 +27,7 @@ namespace arbitrator */ struct VehicleState { - ros::Time stamp; // Time stamp of position data used to populate struct + rclcpp::Time stamp; // Time stamp of position data used to populate struct double x = 0; // Vehicle x axis position in map frame (m) double y = 0; // Vehicle y axis position in map frame (m) double downtrack = 0; // Vehicle route downtrack (m) diff --git a/arbitrator/launch/arbitrator.launch b/arbitrator/launch/arbitrator.launch deleted file mode 100644 index 2f01e4efd3..0000000000 --- a/arbitrator/launch/arbitrator.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - diff --git a/arbitrator/launch/arbitrator.launch.py b/arbitrator/launch/arbitrator.launch.py new file mode 100644 index 0000000000..7b479c6ac3 --- /dev/null +++ b/arbitrator/launch/arbitrator.launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA arbitrator. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='DEBUG') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('arbitrator'), 'config/arbitrator_params.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='arbitrator_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='arbitrator', + plugin='arbitrator::ArbitratorNode', + name='arbitrator_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/arbitrator/package.xml b/arbitrator/package.xml index 9a335c0196..7c8051eb49 100644 --- a/arbitrator/package.xml +++ b/arbitrator/package.xml @@ -1,9 +1,22 @@ arbitrator - 3.3.0 + 4.0.0 The arbitrator package + + @@ -15,22 +28,30 @@ Apache 2.0 - catkin + ament_cmake carma_cmake_common - carma_utils - cav_msgs - cav_srvs - roscpp + ament_auto_cmake + + carma_ros2_utils + carma_planning_msgs + rclcpp lanelet2_core - carma_wm + carma_wm_ros2 tf tf2 tf2_geometry_msgs + rapidjson + + ament_lint_auto + ament_cmake_gtest + ament_cmake_gmock - - - + launch + launch_ros + + ament_cmake + diff --git a/arbitrator/src/arbitrator.cpp b/arbitrator/src/arbitrator.cpp index 58f5b45e61..40949da3aa 100644 --- a/arbitrator/src/arbitrator.cpp +++ b/arbitrator/src/arbitrator.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -15,10 +15,10 @@ */ #include "arbitrator.hpp" -#include -#include +#include +#include #include "arbitrator_utils.hpp" -#include +#include #include #include @@ -26,70 +26,75 @@ namespace arbitrator { void Arbitrator::run() { - ROS_INFO("Aribtrator started, beginning arbitrator state machine."); - while (!ros::isShuttingDown()) + if (!planning_in_progress_ && nh_->get_current_state().id() != lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN) { - ros::spinOnce(); + planning_in_progress_ = true; + switch (sm_->get_state()) { case INITIAL: - ROS_INFO("Aribtrator spinning in INITIAL state."); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in INITIAL state."); initial_state(); break; case PLANNING: - ROS_INFO("Aribtrator spinning in PLANNING state."); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in PLANNING state."); planning_state(); break; case WAITING: - ROS_INFO("Aribtrator spinning in WAITING state."); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in WAITING state."); waiting_state(); break; case PAUSED: - ROS_INFO("Aribtrator spinning in PAUSED state."); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator spinning in PAUSED state."); paused_state(); break; case SHUTDOWN: - ROS_INFO("Arbitrator shutting down after being commanded to shutdown!"); - ros::shutdown(); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator shutting down after being commanded to shutdown!"); + rclcpp::shutdown(); exit(0); break; default: throw std::invalid_argument("State machine attempting to process an illegal state value"); } } + else + { + return; + } + planning_in_progress_ = false; } - void Arbitrator::guidance_state_cb(const cav_msgs::GuidanceState::ConstPtr& msg) + void Arbitrator::guidance_state_cb(carma_planning_msgs::msg::GuidanceState::UniquePtr msg) { switch (msg->state) { - case cav_msgs::GuidanceState::STARTUP: + case carma_planning_msgs::msg::GuidanceState::STARTUP: // NO-OP break; - case cav_msgs::GuidanceState::DRIVERS_READY: + case carma_planning_msgs::msg::GuidanceState::DRIVERS_READY: if(sm_->get_state() == ArbitratorState::PLANNING || sm_->get_state() == ArbitratorState::WAITING) { - ROS_INFO("Received notice that guidance has been restarted, pausing arbitrator."); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Received notice that guidance has been restarted, pausing arbitrator."); sm_->submit_event(ArbitratorEvent::ARBITRATOR_PAUSED); } break; - case cav_msgs::GuidanceState::ACTIVE: + case carma_planning_msgs::msg::GuidanceState::ACTIVE: // NO-OP break; - case cav_msgs::GuidanceState::ENGAGED: - ROS_INFO("Received notice that guidance has been engaged!"); + case carma_planning_msgs::msg::GuidanceState::ENGAGED: + RCLCPP_INFO_STREAM(nh_->get_logger(), "Received notice that guidance has been engaged!"); if (sm_->get_state() == ArbitratorState::INITIAL) { sm_->submit_event(ArbitratorEvent::SYSTEM_STARTUP_COMPLETE); } else if (sm_->get_state() == ArbitratorState::PAUSED) { sm_->submit_event(ArbitratorEvent::ARBITRATOR_RESUMED); } break; - case cav_msgs::GuidanceState::INACTIVE: - ROS_INFO("Received notice that guidance has been disengaged, pausing arbitrator."); + case carma_planning_msgs::msg::GuidanceState::INACTIVE: + RCLCPP_INFO_STREAM(nh_->get_logger(), "Received notice that guidance has been disengaged, pausing arbitrator."); sm_->submit_event(ArbitratorEvent::ARBITRATOR_PAUSED); break; - case cav_msgs::GuidanceState::SHUTDOWN: - ROS_INFO("Received notice that guidance has been shutdown, shutting down arbitrator."); + case carma_planning_msgs::msg::GuidanceState::SHUTDOWN: + RCLCPP_INFO_STREAM(nh_->get_logger(), "Received notice that guidance has been shutdown, shutting down arbitrator."); sm_->submit_event(ArbitratorEvent::SYSTEM_SHUTDOWN_INITIATED); break; default: @@ -101,40 +106,42 @@ namespace arbitrator { if(!initialized_) { - ROS_INFO("Arbitrator initializing on first initial state spin..."); - final_plan_pub_ = nh_->advertise("final_maneuver_plan", 5); - guidance_state_sub_ = nh_->subscribe("guidance_state", 5, &Arbitrator::guidance_state_cb, this); + 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)); + final_plan_pub_->on_activate(); + initialized_ = true; - // TODO: load plan duration from parameters file } - ros::Duration(0.1).sleep(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } void Arbitrator::planning_state() { - ROS_INFO("Aribtrator beginning planning process!"); - ros::Time planning_process_start = ros::Time::now(); - cav_msgs::ManeuverPlan plan = planning_strategy_.generate_plan(vehicle_state_); + 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()) { - ros::Time plan_end_time = arbitrator_utils::get_plan_end_time(plan); - ros::Time plan_start_time = arbitrator_utils::get_plan_start_time(plan); - ros::Duration plan_duration = plan_end_time - plan_start_time; + 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_) { - ROS_WARN_STREAM("Arbitrator is unable to generate a plan with minimum plan duration requirement!"); + RCLCPP_WARN_STREAM(nh_->get_logger(), "Arbitrator is unable to generate a plan with minimum plan duration requirement!"); } else { - ROS_INFO_STREAM("Arbitrator is publishing plan " << plan.maneuver_plan_id << " of duration " << plan_duration << " as current maneuver plan"); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator is publishing plan " << plan.maneuver_plan_id << " of duration " << plan_duration.seconds() << " as current maneuver plan"); } - final_plan_pub_.publish(plan); + final_plan_pub_->publish(plan); } else { - ROS_WARN("Arbitrator was unable to generate a plan!"); + RCLCPP_WARN_STREAM(nh_->get_logger(), "Arbitrator was unable to generate a plan!"); } next_planning_process_start_ = planning_process_start + time_between_plans_; @@ -146,36 +153,36 @@ namespace arbitrator { // Sleep in 100ms increments until our next planning cycle // This ensures we spin() at least a few times - while (ros::Time::now() < next_planning_process_start_) + while (nh_->get_clock()->now() < next_planning_process_start_) { - ros::Duration(0.1).sleep(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - ROS_INFO("Arbitrator transitioning from WAITING to PLANNING state."); + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator transitioning from WAITING to PLANNING state."); sm_->submit_event(ArbitratorEvent::PLANNING_TIMER_TRIGGER); } void Arbitrator::paused_state() { - ros::Duration(0.1).sleep(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } void Arbitrator::shutdown_state() { - ROS_INFO_STREAM("Arbitrator shutting down..."); - ros::shutdown(); // Will stop upper level spin and shutdown node + RCLCPP_INFO_STREAM(nh_->get_logger(), "Arbitrator shutting down..."); + rclcpp::shutdown(); // Will stop upper level spin and shutdown node } void Arbitrator::bumper_pose_cb() { try { - tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", ros::Time(0), ros::Duration(1.0)); //save to local copy of transform 1 sec timeout + tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(1.0, 0)); //save to local copy of transform 1 sec timeout tf2::fromMsg(tf_, bumper_transform_); vehicle_state_.stamp = tf_.header.stamp; } catch (const tf2::TransformException &ex) { - ROS_WARN("%s", ex.what()); + RCLCPP_WARN_STREAM(nh_->get_logger(), ex.what()); } vehicle_state_.x = bumper_transform_.getOrigin().getX(); @@ -189,7 +196,7 @@ namespace arbitrator if (lanelets.empty()) { - ROS_WARN_STREAM("Vehicle is not in a lanelet."); + RCLCPP_WARN_STREAM(nh_->get_logger(), "Vehicle is not in a lanelet."); vehicle_state_.lane_id = lanelet::InvalId; } else { @@ -200,7 +207,7 @@ namespace arbitrator } - void Arbitrator::twist_cb(const geometry_msgs::TwistStampedConstPtr& msg) + void Arbitrator::twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg) { vehicle_state_.velocity = msg->twist.linear.x; } diff --git a/arbitrator/src/arbitrator_node.cpp b/arbitrator/src/arbitrator_node.cpp index d8907a4531..f249736d1d 100644 --- a/arbitrator/src/arbitrator_node.cpp +++ b/arbitrator/src/arbitrator_node.cpp @@ -1,11 +1,12 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -13,84 +14,127 @@ * the License. */ -#include -#include -#include -#include -#include -#include -#include "arbitrator.hpp" -#include "arbitrator_state_machine.hpp" -#include "cost_system_cost_function.hpp" -#include "fixed_priority_cost_function.hpp" -#include "plugin_neighbor_generator.hpp" -#include "beam_search_strategy.hpp" -#include "tree_planner.hpp" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "arbitrator"); - ros::CARMANodeHandle nh = ros::CARMANodeHandle(); - ros::CARMANodeHandle pnh = ros::CARMANodeHandle("~"); - - // Handle dependency injection - arbitrator::CapabilitiesInterface ci{&nh}; - arbitrator::ArbitratorStateMachine sm; - - bool use_fixed_costs = false; - pnh.getParam("use_fixed_costs", use_fixed_costs); - - arbitrator::CostFunction *cf = nullptr; - arbitrator::CostSystemCostFunction cscf = arbitrator::CostSystemCostFunction{}; - std::map plugin_priorities; - pnh.getParam("plugin_priorities", plugin_priorities); - arbitrator::FixedPriorityCostFunction fpcf{plugin_priorities}; - if (use_fixed_costs) { - cf = &fpcf; - } else { - cscf.init(nh); - cf = &cscf; - } - - int beam_width; - pnh.param("beam_width", beam_width, 3); - arbitrator::BeamSearchStrategy bss{beam_width}; - - arbitrator::PluginNeighborGenerator png{ci}; +#include "arbitrator_node.hpp" - double target_plan; - pnh.param("target_plan_duration", target_plan, 15.0); - arbitrator::TreePlanner tp{*cf, png, bss, ros::Duration(target_plan)}; - double min_plan_duration; - pnh.param("min_plan_duration", min_plan_duration, 6.0); - - double planning_frequency; - pnh.param("planning_frequency", planning_frequency, 1.0); - - carma_wm::WMListener wml; - auto wm = wml.getWorldModel(); - - arbitrator::Arbitrator arbitrator{ - &nh, - &pnh, - &sm, - &ci, - tp, - ros::Duration(min_plan_duration), - ros::Rate(planning_frequency), - wm }; +namespace arbitrator +{ + ArbitratorNode::ArbitratorNode(const rclcpp::NodeOptions& options) : carma_ros2_utils::CarmaLifecycleNode(options) + { + // Create initial config + config_ = Config(); + + config_.min_plan_duration = declare_parameter("min_plan_duration", config_.min_plan_duration); + config_.target_plan_duration = declare_parameter("target_plan_duration", config_.target_plan_duration); + config_.planning_frequency = declare_parameter("planning_frequency", config_.planning_frequency); + config_.beam_width = declare_parameter("beam_width", config_.beam_width); + 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 + arbitrator::CapabilitiesInterface ci(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); + get_parameter("beam_width", config_.beam_width); + get_parameter("use_fixed_costs", config_.use_fixed_costs); + std::string json_string; + get_parameter("plugin_priorities", json_string); + + config_.plugin_priorities = plugin_priorities_map_from_json(json_string); + + RCLCPP_INFO_STREAM(get_logger(), "Arbitrator Loaded Params: " << config_); + + std::shared_ptr cf; + arbitrator::CostSystemCostFunction cscf = arbitrator::CostSystemCostFunction(); + + arbitrator::FixedPriorityCostFunction fpcf(config_.plugin_priorities); + if (config_.use_fixed_costs) { + cf = std::make_shared(fpcf); + } else { + cscf.init(shared_from_this()); + cf = std::make_shared(cscf); + } + + auto bss = std::make_shared(config_.beam_width); + + auto png = std::make_shared>(ci); + arbitrator::TreePlanner tp(cf, png, bss, rclcpp::Duration(config_.target_plan_duration* 1e9)); + + auto 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() + ); + + auto wm = wm_listener_->getWorldModel(); + + arbitrator_ = std::make_shared( + shared_from_this(), + std::make_shared(sm), + std::make_shared(ci), + std::make_shared(tp), + rclcpp::Duration(config_.min_plan_duration* 1e9), + 1/config_.planning_frequency, + wm ); + + + carma_ros2_utils::SubPtr 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), + [this]() {this->arbitrator_->run();}); + RCLCPP_INFO_STREAM(get_logger(), "Arbitrator started, beginning arbitrator state machine."); + return CallbackReturn::SUCCESS; + } + std::map ArbitratorNode::plugin_priorities_map_from_json(const std::string& json_string) + { + std::map map; - ros::Subscriber twist_sub = nh.subscribe("current_velocity", 1, &arbitrator::Arbitrator::twist_cb, &arbitrator); + rapidjson::Document d; + if(d.Parse(json_string.c_str()).HasParseError()) + { + RCLCPP_WARN(get_logger(), "Failed to parse plugin_priorities map. Invalid json structure"); + return map; + } + if (!d.HasMember("plugin_priorities")) { + RCLCPP_WARN(get_logger(), "No plugin_priorities found in arbitrator config"); + return map; + } + rapidjson::Value& map_value = d["plugin_priorities"]; + + for (rapidjson::Value::ConstMemberIterator it = map_value.MemberBegin(); + it != map_value.MemberEnd(); it++) + { + map[it->name.GetString()] = it->value.GetDouble(); + } + return map; + + } - arbitrator.initializeBumperTransformLookup(); +}; - ros::Timer bumper_pose_timer = nh.createTimer( - ros::Duration(ros::Rate(10.0)), - [&arbitrator](const auto&) {arbitrator.bumper_pose_cb();}); - arbitrator.run(); +#include "rclcpp_components/register_node_macro.hpp" - return 0; -} +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(arbitrator::ArbitratorNode) \ No newline at end of file diff --git a/arbitrator/src/arbitrator_state_machine.cpp b/arbitrator/src/arbitrator_state_machine.cpp index 3e753f82d3..842263a8c7 100644 --- a/arbitrator/src/arbitrator_state_machine.cpp +++ b/arbitrator/src/arbitrator_state_machine.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -15,6 +15,7 @@ */ #include "arbitrator_state_machine.hpp" +#include namespace arbitrator { @@ -27,12 +28,13 @@ namespace arbitrator ArbitratorState ArbitratorStateMachine::submit_event(ArbitratorEvent event) { - for (auto iter = ARBITRATOR_TRANSITIONS.begin(); iter != ARBITRATOR_TRANSITIONS.end(); iter++) + for (auto pair : ARBITRATOR_TRANSITIONS) { - if (current_state == iter->current_state && event == iter->input_event) { + if (current_state == pair.current_state && event == pair.input_event) + { // In the event of multiple legal transistions this will take the last transition, // But ultimately this condition should never arise. - current_state = iter->final_state; + current_state = pair.final_state; break; } } diff --git a/arbitrator/src/arbitrator_utils.cpp b/arbitrator/src/arbitrator_utils.cpp index d71f22624f..8e6622b150 100644 --- a/arbitrator/src/arbitrator_utils.cpp +++ b/arbitrator/src/arbitrator_utils.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -15,73 +15,73 @@ */ #include "arbitrator_utils.hpp" -#include +#include #include namespace arbitrator_utils { - ros::Time get_plan_end_time(const cav_msgs::ManeuverPlan &plan) + rclcpp::Time get_plan_end_time(const carma_planning_msgs::msg::ManeuverPlan &plan) { if (plan.maneuvers.empty()) { throw std::invalid_argument("arbitrator::get_plan_end_time called on empty maneuver plan"); } - cav_msgs::Maneuver m = plan.maneuvers.back(); + carma_planning_msgs::msg::Maneuver m = plan.maneuvers.back(); return get_maneuver_end_time(m); } - double get_plan_end_distance(const cav_msgs::ManeuverPlan &plan) + double get_plan_end_distance(const carma_planning_msgs::msg::ManeuverPlan &plan) { if (plan.maneuvers.empty()) { throw std::invalid_argument("arbitrator::get_plan_end_dist called on empty maneuver plan"); } - cav_msgs::Maneuver m = plan.maneuvers.back(); + carma_planning_msgs::msg::Maneuver m = plan.maneuvers.back(); return get_maneuver_end_distance(m); } - ros::Time get_plan_start_time(const cav_msgs::ManeuverPlan &plan) + rclcpp::Time get_plan_start_time(const carma_planning_msgs::msg::ManeuverPlan &plan) { if (plan.maneuvers.empty()) { throw std::invalid_argument("arbitrator::get_plan_start_time called on empty maneuver plan"); } - cav_msgs::Maneuver m = plan.maneuvers.front(); + carma_planning_msgs::msg::Maneuver m = plan.maneuvers.front(); return get_maneuver_start_time(m); } - double get_plan_start_distance(const cav_msgs::ManeuverPlan &plan) + double get_plan_start_distance(const carma_planning_msgs::msg::ManeuverPlan &plan) { if (plan.maneuvers.empty()) { throw std::invalid_argument("arbitrator::get_plan_start_dist called on empty maneuver plan"); } - cav_msgs::Maneuver m = plan.maneuvers.front(); + carma_planning_msgs::msg::Maneuver m = plan.maneuvers.front(); return get_maneuver_start_distance(m); } - ros::Time get_maneuver_end_time(const cav_msgs::Maneuver &mvr) + rclcpp::Time get_maneuver_end_time(const carma_planning_msgs::msg::Maneuver &mvr) { return GET_MANEUVER_PROPERTY(mvr, end_time); } - ros::Time get_maneuver_start_time(const cav_msgs::Maneuver &mvr) + rclcpp::Time get_maneuver_start_time(const carma_planning_msgs::msg::Maneuver &mvr) { return GET_MANEUVER_PROPERTY(mvr, start_time); } - double get_maneuver_end_distance(const cav_msgs::Maneuver &mvr) + double get_maneuver_end_distance(const carma_planning_msgs::msg::Maneuver &mvr) { return GET_MANEUVER_PROPERTY(mvr, end_dist); } - double get_maneuver_start_distance(const cav_msgs::Maneuver &mvr) + double get_maneuver_start_distance(const carma_planning_msgs::msg::Maneuver &mvr) { return GET_MANEUVER_PROPERTY(mvr, start_dist); } diff --git a/arbitrator/src/beam_search_strategy.cpp b/arbitrator/src/beam_search_strategy.cpp index b07474984b..4743d79581 100644 --- a/arbitrator/src/beam_search_strategy.cpp +++ b/arbitrator/src/beam_search_strategy.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -18,11 +18,11 @@ namespace arbitrator { - std::vector> BeamSearchStrategy::prioritize_plans(std::vector> plans) const + std::vector> BeamSearchStrategy::prioritize_plans(std::vector> plans) const { std::sort(plans.begin(), plans.end(), - [] (const std::pair& a, const std::pair& b) + [] (const std::pair& a, const std::pair& b) { return a.second < b.second; } diff --git a/arbitrator/src/capabilities_interface.cpp b/arbitrator/src/capabilities_interface.cpp index 70d0bedb61..29d0c4a99b 100644 --- a/arbitrator/src/capabilities_interface.cpp +++ b/arbitrator/src/capabilities_interface.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -15,7 +15,7 @@ */ #include "capabilities_interface.hpp" -#include +#include #include #include @@ -27,12 +27,16 @@ namespace arbitrator { std::vector topics = {}; - cav_srvs::GetPluginApi srv; - srv.request.capability = ""; + auto srv = std::make_shared(); + srv->capability = ""; + + auto plan_response = sc_s_->async_send_request(srv); - if (query_string == STRATEGIC_PLAN_CAPABILITY && sc_s.call(srv)) + auto future_status = plan_response.wait_for(std::chrono::milliseconds(200)); + + if (query_string == STRATEGIC_PLAN_CAPABILITY && future_status == std::future_status::ready) { - topics = srv.response.plan_service; + topics = plan_response.get()->plan_service; // Log the topics std::ostringstream stream; @@ -41,12 +45,12 @@ namespace arbitrator stream << topic << ", "; } stream << std::endl; - ROS_INFO(stream.str().c_str()); + RCLCPP_INFO_STREAM(nh_->get_logger(), stream.str().c_str()); } else { - ROS_DEBUG_STREAM("servicd call failed..."); - ROS_DEBUG_STREAM("client: " << sc_s); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "service call failed..."); + RCLCPP_ERROR_STREAM(nh_->get_logger(), "client: " << sc_s_); } return topics; diff --git a/arbitrator/src/cost_system_cost_function.cpp b/arbitrator/src/cost_system_cost_function.cpp index e3ffaf293d..aaa58aff8b 100644 --- a/arbitrator/src/cost_system_cost_function.cpp +++ b/arbitrator/src/cost_system_cost_function.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -16,19 +16,19 @@ #include "cost_system_cost_function.hpp" #include "arbitrator_utils.hpp" -#include "cav_srvs/ComputePlanCost.h" -#include "cav_msgs/ManeuverParameters.h" +#include +#include #include namespace arbitrator { - void CostSystemCostFunction::init(ros::NodeHandle &nh) + void CostSystemCostFunction::init(std::shared_ptr nh) { - cost_system_sc_ = nh.serviceClient("compute_plan_cost"); + cost_system_sc_ = nh->create_client("compute_plan_cost"); initialized_ = true; } - double CostSystemCostFunction::compute_total_cost(const cav_msgs::ManeuverPlan& plan) + double CostSystemCostFunction::compute_total_cost(const carma_planning_msgs::msg::ManeuverPlan& plan) { if (!initialized_) { throw std::logic_error("Attempt to use CostSystemCostFunction before initialization."); @@ -36,19 +36,23 @@ namespace arbitrator double total_cost = std::numeric_limits::infinity(); - cav_srvs::ComputePlanCost service_message; - service_message.request.maneuver_plan = plan; + auto service_message = std::make_shared(); + service_message->maneuver_plan = plan; + + auto resp = cost_system_sc_->async_send_request(service_message); - if (cost_system_sc_.call(service_message)){ - total_cost = service_message.response.plan_cost; + auto future_status = resp.wait_for(std::chrono::milliseconds(500)); + + if (future_status == std::future_status::ready){ + total_cost = resp.get()->plan_cost; } else { - ROS_WARN_STREAM("Unable to get cost for plan from CostPluginSystem due to service call failure."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "Unable to get cost for plan from CostPluginSystem due to service call failure."); } return total_cost; } - double CostSystemCostFunction::compute_cost_per_unit_distance(const cav_msgs::ManeuverPlan& plan) + double CostSystemCostFunction::compute_cost_per_unit_distance(const carma_planning_msgs::msg::ManeuverPlan& plan) { double plan_dist = arbitrator_utils::get_plan_end_distance(plan) - arbitrator_utils::get_plan_start_distance(plan); return compute_total_cost(plan) / plan_dist; diff --git a/arbitrator/src/fixed_priority_cost_function.cpp b/arbitrator/src/fixed_priority_cost_function.cpp index 00c70f14ac..0c4881c7c7 100644 --- a/arbitrator/src/fixed_priority_cost_function.cpp +++ b/arbitrator/src/fixed_priority_cost_function.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -16,8 +16,8 @@ #include "fixed_priority_cost_function.hpp" #include "arbitrator_utils.hpp" -#include "cav_msgs/ManeuverParameters.h" -#include +#include +#include #include namespace arbitrator @@ -38,11 +38,11 @@ namespace arbitrator for (auto it = plugin_priorities.begin(); it != plugin_priorities.end(); it++) { plugin_costs_[it->first] = 1.0 - (it->second / max_priority); - ROS_INFO_STREAM("FixedPriorityCostFunction: " << "Plugin: " << it->first << " Normalized Cost: " << plugin_costs_[it->first]); + RCLCPP_INFO_STREAM(rclcpp::get_logger("arbitrator"), "FixedPriorityCostFunction: " << "Plugin: " << it->first << " Normalized Cost: " << plugin_costs_[it->first]); } } - double FixedPriorityCostFunction::compute_total_cost(const cav_msgs::ManeuverPlan& plan) + double FixedPriorityCostFunction::compute_total_cost(const carma_planning_msgs::msg::ManeuverPlan& plan) { double total_cost = 0.0; for (auto it = plan.maneuvers.begin(); it != plan.maneuvers.end(); it++) @@ -55,7 +55,7 @@ namespace arbitrator return total_cost; } - double FixedPriorityCostFunction::compute_cost_per_unit_distance(const cav_msgs::ManeuverPlan& plan) + double FixedPriorityCostFunction::compute_cost_per_unit_distance(const carma_planning_msgs::msg::ManeuverPlan& plan) { double plan_dist = arbitrator_utils::get_plan_end_distance(plan) - arbitrator_utils::get_plan_start_distance(plan); return compute_total_cost(plan) / plan_dist; diff --git a/arbitrator/src/main.cpp b/arbitrator/src/main.cpp new file mode 100644 index 0000000000..d0a26171b5 --- /dev/null +++ b/arbitrator/src/main.cpp @@ -0,0 +1,35 @@ +/* + * 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. + */ + +#include "arbitrator_node.hpp" +#include + + +int main(int argc, char** argv) +{ + // Initialize node + rclcpp::init(argc, argv); + + // Call current CARMA arbitrator node instance called ArbitratorNode with specific planning paradigm. + 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/arbitrator/src/tree_planner.cpp b/arbitrator/src/tree_planner.cpp index 85cc44a6e5..eef02863c7 100644 --- a/arbitrator/src/tree_planner.cpp +++ b/arbitrator/src/tree_planner.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -22,37 +22,37 @@ namespace arbitrator { - cav_msgs::ManeuverPlan TreePlanner::generate_plan(const VehicleState& start_state) + carma_planning_msgs::msg::ManeuverPlan TreePlanner::generate_plan(const VehicleState& start_state) { - cav_msgs::ManeuverPlan root; - std::vector> open_list_to_evaluate; - std::vector> final_open_list; + carma_planning_msgs::msg::ManeuverPlan root; + std::vector> open_list_to_evaluate; + std::vector> final_open_list; const double INF = std::numeric_limits::infinity(); open_list_to_evaluate.push_back(std::make_pair(root, INF)); - cav_msgs::ManeuverPlan longest_plan = root; // Track longest plan in case target length is never reached - ros::Duration longest_plan_duration = ros::Duration(0); + carma_planning_msgs::msg::ManeuverPlan longest_plan = root; // Track longest plan in case target length is never reached + rclcpp::Duration longest_plan_duration = rclcpp::Duration(0); while (!open_list_to_evaluate.empty()) { - std::vector> temp_open_list; + std::vector> temp_open_list; for (auto it = open_list_to_evaluate.begin(); it != open_list_to_evaluate.end(); it++) { // Pop the first element off the open list - cav_msgs::ManeuverPlan cur_plan = it->first; + carma_planning_msgs::msg::ManeuverPlan cur_plan = it->first; - ROS_DEBUG_STREAM("START"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "START"); for (auto mvr : cur_plan.maneuvers) { - ROS_DEBUG_STREAM("Printing cur_plan: mvr: "<< (int)mvr.type); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "Printing cur_plan: mvr: "<< (int)mvr.type); } - ROS_DEBUG_STREAM("PRINT END"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "PRINT END"); - ros::Duration plan_duration; // zero duration + auto plan_duration = rclcpp::Duration(0, 0); // zero duration // If we're not at the root, plan_duration is nonzero (our plan should have maneuvers) if (!cur_plan.maneuvers.empty()) @@ -72,40 +72,42 @@ namespace arbitrator if (plan_duration >= target_plan_duration_) { final_open_list.push_back((*it)); - ROS_DEBUG_STREAM("Has enough duration, skipping that which has following mvrs..:"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "Has enough duration, skipping that which has following mvrs..:"); for (auto mvr : it->first.maneuvers) { - ROS_DEBUG_STREAM("Printing mvr: "<< (int)mvr.type); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "Printing mvr: "<< (int)mvr.type); } continue; } - - // Expand it, and reprioritize - std::vector children = neighbor_generator_.generate_neighbors(cur_plan, start_state); + // Expand it, and reprioritize + std::vector children = neighbor_generator_->generate_neighbors(cur_plan, start_state); + // Compute cost for each child and store in open list for (auto child = children.begin(); child != children.end(); child++) { + if (child->maneuvers.empty()) { - ROS_DEBUG_STREAM("Child was empty for id: " << child->maneuver_plan_id); + RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "Child was empty for id: " << child->maneuver_plan_id); continue; } - temp_open_list.push_back(std::make_pair(*child, cost_function_.compute_cost_per_unit_distance(*child))); - } + temp_open_list.push_back(std::make_pair(*child, cost_function_->compute_cost_per_unit_distance(*child))); + } } + open_list_to_evaluate = temp_open_list; } - final_open_list = search_strategy_.prioritize_plans(final_open_list); - + final_open_list = search_strategy_->prioritize_plans(final_open_list); + // now every plan has enough duration if possible and prioritized for (auto pair : final_open_list) { // Pop the first element off the open list - cav_msgs::ManeuverPlan cur_plan = pair.first; - ros::Duration plan_duration; // zero duration + carma_planning_msgs::msg::ManeuverPlan cur_plan = pair.first; + rclcpp::Duration plan_duration(0,0); // zero duration // get plan duration plan_duration = arbitrator_utils::get_plan_end_time(cur_plan) - arbitrator_utils::get_plan_start_time(cur_plan); diff --git a/arbitrator/test/test_arbitrator_state_machine.cpp b/arbitrator/test/test_arbitrator_state_machine.cpp index d3e04f7b75..161cf61af8 100644 --- a/arbitrator/test/test_arbitrator_state_machine.cpp +++ b/arbitrator/test/test_arbitrator_state_machine.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 diff --git a/arbitrator/test/test_beam_search_strategy.cpp b/arbitrator/test/test_beam_search_strategy.cpp index 22b8847d72..ada818779f 100644 --- a/arbitrator/test/test_beam_search_strategy.cpp +++ b/arbitrator/test/test_beam_search_strategy.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -20,7 +20,7 @@ namespace arbitrator { TEST_F(BeamSearchStrategyTest, testEmptyList) { - auto vec = bss.prioritize_plans(std::vector>()); + auto vec = bss.prioritize_plans(std::vector>()); ASSERT_EQ(0, vec.size()); ASSERT_TRUE(vec.empty()); } @@ -28,10 +28,10 @@ namespace arbitrator TEST_F(BeamSearchStrategyTest, testSort1) { auto vec = bss.prioritize_plans(std::vector< - std::pair>({ - { cav_msgs::ManeuverPlan(), 10.0 }, - { cav_msgs::ManeuverPlan(), 5.0 }, - { cav_msgs::ManeuverPlan(), 0.0 }, + std::pair>({ + { carma_planning_msgs::msg::ManeuverPlan(), 10.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 5.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 0.0 }, })); ASSERT_EQ(3, vec.size()); @@ -44,10 +44,10 @@ namespace arbitrator TEST_F(BeamSearchStrategyTest, testSort2) { auto vec = bss.prioritize_plans(std::vector< - std::pair>({ - { cav_msgs::ManeuverPlan(), 10.0 }, - { cav_msgs::ManeuverPlan(), 0.0 }, - { cav_msgs::ManeuverPlan(), 5.0 }, + std::pair>({ + { carma_planning_msgs::msg::ManeuverPlan(), 10.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 0.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 5.0 }, })); ASSERT_EQ(3, vec.size()); @@ -60,10 +60,10 @@ namespace arbitrator TEST_F(BeamSearchStrategyTest, testSort3) { auto vec = bss.prioritize_plans(std::vector< - std::pair>({ - { cav_msgs::ManeuverPlan(), 0.0 }, - { cav_msgs::ManeuverPlan(), 5.0 }, - { cav_msgs::ManeuverPlan(), 10.0 }, + std::pair>({ + { carma_planning_msgs::msg::ManeuverPlan(), 0.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 5.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 10.0 }, })); ASSERT_EQ(3, vec.size()); @@ -76,18 +76,18 @@ namespace arbitrator TEST_F(BeamSearchStrategyTest, testSortAndPrune1) { auto vec = bss.prioritize_plans(std::vector< - std::pair>({ - { cav_msgs::ManeuverPlan(), 10.0 }, - { cav_msgs::ManeuverPlan(), 5.0 }, - { cav_msgs::ManeuverPlan(), 3.0 }, - { cav_msgs::ManeuverPlan(), 7.0 }, - { cav_msgs::ManeuverPlan(), 6.0 }, - { cav_msgs::ManeuverPlan(), 1.0 }, - { cav_msgs::ManeuverPlan(), 9.0 }, - { cav_msgs::ManeuverPlan(), 8.0 }, - { cav_msgs::ManeuverPlan(), 2.0 }, - { cav_msgs::ManeuverPlan(), 4.0 }, - { cav_msgs::ManeuverPlan(), 0.0 }, + std::pair>({ + { carma_planning_msgs::msg::ManeuverPlan(), 10.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 5.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 3.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 7.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 6.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 1.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 9.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 8.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 2.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 4.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 0.0 }, })); ASSERT_EQ(3, vec.size()); @@ -100,18 +100,18 @@ namespace arbitrator TEST_F(BeamSearchStrategyTest, testSortAndPrune2) { auto vec = bss.prioritize_plans(std::vector< - std::pair>({ - { cav_msgs::ManeuverPlan(), 10.0 }, - { cav_msgs::ManeuverPlan(), 9.0 }, - { cav_msgs::ManeuverPlan(), 8.0 }, - { cav_msgs::ManeuverPlan(), 7.0 }, - { cav_msgs::ManeuverPlan(), 6.0 }, - { cav_msgs::ManeuverPlan(), 5.0 }, - { cav_msgs::ManeuverPlan(), 4.0 }, - { cav_msgs::ManeuverPlan(), 3.0 }, - { cav_msgs::ManeuverPlan(), 2.0 }, - { cav_msgs::ManeuverPlan(), 1.0 }, - { cav_msgs::ManeuverPlan(), 0.0 }, + std::pair>({ + { carma_planning_msgs::msg::ManeuverPlan(), 10.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 9.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 8.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 7.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 6.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 5.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 4.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 3.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 2.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 1.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 0.0 }, })); ASSERT_EQ(3, vec.size()); @@ -124,18 +124,18 @@ namespace arbitrator TEST_F(BeamSearchStrategyTest, testSortAndPrune3) { auto vec = bss.prioritize_plans(std::vector< - std::pair>({ - { cav_msgs::ManeuverPlan(), 0.0 }, - { cav_msgs::ManeuverPlan(), 1.0 }, - { cav_msgs::ManeuverPlan(), 2.0 }, - { cav_msgs::ManeuverPlan(), 3.0 }, - { cav_msgs::ManeuverPlan(), 4.0 }, - { cav_msgs::ManeuverPlan(), 5.0 }, - { cav_msgs::ManeuverPlan(), 6.0 }, - { cav_msgs::ManeuverPlan(), 7.0 }, - { cav_msgs::ManeuverPlan(), 8.0 }, - { cav_msgs::ManeuverPlan(), 9.0 }, - { cav_msgs::ManeuverPlan(), 10.0 }, + std::pair>({ + { carma_planning_msgs::msg::ManeuverPlan(), 0.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 1.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 2.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 3.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 4.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 5.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 6.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 7.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 8.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 9.0 }, + { carma_planning_msgs::msg::ManeuverPlan(), 10.0 }, })); ASSERT_EQ(3, vec.size()); diff --git a/arbitrator/test/test_fixed_priority_cost_function.cpp b/arbitrator/test/test_fixed_priority_cost_function.cpp index 5174af55a4..0c21ea447d 100644 --- a/arbitrator/test/test_fixed_priority_cost_function.cpp +++ b/arbitrator/test/test_fixed_priority_cost_function.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * Copyright (C) 2022 LEIDOS. * * Licensed under the Apache License, Version 2.0 (the "License"); you may not * use this file except in compliance with the License. You may obtain a copy of @@ -21,15 +21,14 @@ namespace arbitrator { TEST_F(FixedPriorityCostFunctionTest, testSingleManeuver) { - ros::Time::init(); - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver mvr1; - mvr1.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::ManeuverPlan plan; + carma_planning_msgs::msg::Maneuver mvr1; + mvr1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; mvr1.lane_following_maneuver.start_dist = 0; - mvr1.lane_following_maneuver.start_time = ros::Time::now(); + mvr1.lane_following_maneuver.start_time = rclcpp::Time(0,0); mvr1.lane_following_maneuver.lane_ids = {"0"}; mvr1.lane_following_maneuver.end_dist = 1; - mvr1.lane_following_maneuver.end_time = ros::Time::now(); + mvr1.lane_following_maneuver.end_time = rclcpp::Time(0,0); mvr1.lane_following_maneuver.start_speed = 10; mvr1.lane_following_maneuver.end_speed = 10; mvr1.lane_following_maneuver.parameters.maneuver_id.push_back(0); @@ -42,27 +41,26 @@ namespace arbitrator TEST_F(FixedPriorityCostFunctionTest, testMixedPlanners) { - ros::Time::init(); - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver mvr1; - mvr1.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::ManeuverPlan plan; + carma_planning_msgs::msg::Maneuver mvr1; + mvr1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; mvr1.lane_following_maneuver.start_dist = 0; - mvr1.lane_following_maneuver.start_time = ros::Time::now(); + mvr1.lane_following_maneuver.start_time = rclcpp::Time(0,0); mvr1.lane_following_maneuver.lane_ids = {"0"}; mvr1.lane_following_maneuver.end_dist = 1; - mvr1.lane_following_maneuver.end_time = ros::Time::now(); + mvr1.lane_following_maneuver.end_time = rclcpp::Time(0,0); mvr1.lane_following_maneuver.start_speed = 10; mvr1.lane_following_maneuver.end_speed = 10; mvr1.lane_following_maneuver.parameters.maneuver_id.push_back(0); mvr1.lane_following_maneuver.parameters.planning_strategic_plugin = "plugin_a"; - cav_msgs::Maneuver mvr2; - mvr2.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver mvr2; + mvr2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; mvr2.lane_following_maneuver.start_dist = 1; - mvr2.lane_following_maneuver.start_time = ros::Time::now(); + mvr2.lane_following_maneuver.start_time = rclcpp::Time(0,0); mvr2.lane_following_maneuver.lane_ids = {"0"}; mvr2.lane_following_maneuver.end_dist = 2; - mvr2.lane_following_maneuver.end_time = ros::Time::now(); + mvr2.lane_following_maneuver.end_time = rclcpp::Time(0,0); mvr2.lane_following_maneuver.start_speed = 10; mvr2.lane_following_maneuver.end_speed = 10; mvr2.lane_following_maneuver.parameters.maneuver_id.push_back(0); @@ -76,15 +74,14 @@ namespace arbitrator TEST_F(FixedPriorityCostFunctionTest, testSingleManeuverUnitDistance) { - ros::Time::init(); - cav_msgs::ManeuverPlan plan1; - cav_msgs::Maneuver mvr1; - mvr1.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::ManeuverPlan plan1; + carma_planning_msgs::msg::Maneuver mvr1; + mvr1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; mvr1.lane_following_maneuver.start_dist = 0; - mvr1.lane_following_maneuver.start_time = ros::Time::now(); + mvr1.lane_following_maneuver.start_time = rclcpp::Time(0,0); mvr1.lane_following_maneuver.lane_ids = {"0"}; mvr1.lane_following_maneuver.end_dist = 1; - mvr1.lane_following_maneuver.end_time = ros::Time::now(); + mvr1.lane_following_maneuver.end_time = rclcpp::Time(0,0); mvr1.lane_following_maneuver.start_speed = 10; mvr1.lane_following_maneuver.end_speed = 10; mvr1.lane_following_maneuver.parameters.maneuver_id.push_back(0); @@ -97,26 +94,25 @@ namespace arbitrator TEST_F(FixedPriorityCostFunctionTest, testUnitDistanceCost) { - ros::Time::init(); - cav_msgs::ManeuverPlan plan2; - cav_msgs::Maneuver mvr2; - mvr2.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::ManeuverPlan plan2; + carma_planning_msgs::msg::Maneuver mvr2; + mvr2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; mvr2.lane_following_maneuver.start_dist = 0; - mvr2.lane_following_maneuver.start_time = ros::Time::now(); + mvr2.lane_following_maneuver.start_time = rclcpp::Time(0,0); mvr2.lane_following_maneuver.lane_ids = {"0"}; mvr2.lane_following_maneuver.end_dist = 1; - mvr2.lane_following_maneuver.end_time = ros::Time::now(); + mvr2.lane_following_maneuver.end_time = rclcpp::Time(0,0); mvr2.lane_following_maneuver.start_speed = 10; mvr2.lane_following_maneuver.end_speed = 10; mvr2.lane_following_maneuver.parameters.maneuver_id.push_back(0); mvr2.lane_following_maneuver.parameters.planning_strategic_plugin = "plugin_a"; - cav_msgs::Maneuver mvr3; - mvr3.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver mvr3; + mvr3.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; mvr3.lane_following_maneuver.start_dist = 1; - mvr3.lane_following_maneuver.start_time = ros::Time::now(); + mvr3.lane_following_maneuver.start_time = rclcpp::Time(0,0); mvr3.lane_following_maneuver.lane_ids = {"0"}; mvr3.lane_following_maneuver.end_dist = 2; - mvr3.lane_following_maneuver.end_time = ros::Time::now(); + mvr3.lane_following_maneuver.end_time = rclcpp::Time(0,0); mvr3.lane_following_maneuver.start_speed = 10; mvr3.lane_following_maneuver.end_speed = 10; mvr3.lane_following_maneuver.parameters.maneuver_id.push_back(0); diff --git a/arbitrator/test/test_main.cpp b/arbitrator/test/test_main.cpp index a68575236b..a8811ac88b 100644 --- a/arbitrator/test/test_main.cpp +++ b/arbitrator/test/test_main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 diff --git a/arbitrator/test/test_plugin_neighbor_generator.cpp b/arbitrator/test/test_plugin_neighbor_generator.cpp index 5aa8f7cd46..8a3f80dc72 100644 --- a/arbitrator/test/test_plugin_neighbor_generator.cpp +++ b/arbitrator/test/test_plugin_neighbor_generator.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -15,31 +15,33 @@ */ #include "test_utils.h" -#include -#include +#include +#include +#include #include #include "plugin_neighbor_generator.hpp" namespace arbitrator { + class MockCapabilitiesInterface { public: - using PluginResponses = std::map; - MOCK_METHOD2(get_plans, PluginResponses(std::string, cav_srvs::PlanManeuvers)); + using PluginResponses = std::map>; + MOCK_METHOD2(get_plans, PluginResponses(std::string, std::shared_ptr)); MOCK_METHOD1(get_topics_for_capability, std::vector(const std::string&)); - template - std::map multiplex_service_call_for_capability(std::string query_string, MSrv msg); + template + std::map> multiplex_service_call_for_capability(std::string query_string, std::shared_ptr msg); ~MockCapabilitiesInterface(){}; }; template<> - std::map + std::map> MockCapabilitiesInterface::multiplex_service_call_for_capability( std::string query_string, - cav_srvs::PlanManeuvers msg) + std::shared_ptr msg) { return get_plans(query_string, msg); } @@ -48,7 +50,7 @@ namespace arbitrator { public: PluginNeighborGeneratorTest(): - png{mci} {}; + png{mci} {}; MockCapabilitiesInterface mci; PluginNeighborGenerator png; @@ -65,11 +67,11 @@ namespace arbitrator get_plans(::testing::_, ::testing::_)) .WillRepeatedly( ::testing::Return( - std::map())); + std::map>())); - cav_msgs::ManeuverPlan plan; + carma_planning_msgs::msg::ManeuverPlan plan; VehicleState vs; - std::vector plans = png.generate_neighbors(plan, vs); + std::vector plans = png.generate_neighbors(plan, vs); ASSERT_EQ(0, plans.size()); ASSERT_TRUE(plans.empty()); @@ -77,29 +79,30 @@ namespace arbitrator TEST_F(PluginNeighborGeneratorTest, testGetNeighbors2) { - std::map responses; - cav_srvs::PlanManeuvers resp1, resp2, resp3; - resp1.response.new_plan.maneuver_plan_id.push_back(0); - resp2.response.new_plan.maneuver_plan_id.push_back(1); - resp3.response.new_plan.maneuver_plan_id.push_back(2); + std::map> responses; + auto resp1 = std::make_shared(); + auto resp2 = std::make_shared(); + auto resp3 = std::make_shared(); + + resp1->new_plan.maneuver_plan_id = "1"; + resp2->new_plan.maneuver_plan_id = "2"; + resp3->new_plan.maneuver_plan_id = "3"; responses["test_plugin_a"] = resp1; responses["test_plugin_b"] = resp2; responses["test_plugin_c"] = resp3; - + EXPECT_CALL(mci, get_plans(::testing::_, ::testing::_)) .WillRepeatedly( ::testing::Return( responses)); - - cav_msgs::ManeuverPlan plan; + carma_planning_msgs::msg::ManeuverPlan plan; VehicleState vs; - std::vector plans = png.generate_neighbors(plan, vs); - + std::vector plans = png.generate_neighbors(plan, vs); ASSERT_FALSE(plans.empty()); ASSERT_EQ(3, plans.size()); - ASSERT_EQ(0, plans[0].maneuver_plan_id[0]); - ASSERT_EQ(1, plans[1].maneuver_plan_id[0]); - ASSERT_EQ(2, plans[2].maneuver_plan_id[0]); + ASSERT_EQ("1", plans[0].maneuver_plan_id); + ASSERT_EQ("2", plans[1].maneuver_plan_id); + ASSERT_EQ("3", plans[2].maneuver_plan_id); } } \ No newline at end of file diff --git a/arbitrator/test/test_tree_planner.cpp b/arbitrator/test/test_tree_planner.cpp index 32d6c87fdb..8a36e90dcf 100644 --- a/arbitrator/test/test_tree_planner.cpp +++ b/arbitrator/test/test_tree_planner.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -16,6 +16,7 @@ #include "test_utils.h" #include "tree_planner.hpp" +#include #include #include "vehicle_state.hpp" @@ -32,7 +33,7 @@ namespace arbitrator class MockSearchStrategy : public SearchStrategy { public: - using PlanAndCost = std::pair; + using PlanAndCost = std::pair; MOCK_CONST_METHOD1(prioritize_plans, std::vector(std::vector)); ~MockSearchStrategy(){}; }; @@ -40,8 +41,8 @@ namespace arbitrator class MockCostFunction : public CostFunction { public: - MOCK_METHOD1(compute_total_cost, double(const cav_msgs::ManeuverPlan&)); - MOCK_METHOD1(compute_cost_per_unit_distance, double(const cav_msgs::ManeuverPlan&)); + MOCK_METHOD1(compute_total_cost, double(const carma_planning_msgs::msg::ManeuverPlan&)); + MOCK_METHOD1(compute_cost_per_unit_distance, double(const carma_planning_msgs::msg::ManeuverPlan&)); ~MockCostFunction(){}; }; @@ -49,7 +50,7 @@ namespace arbitrator class MockNeighborGenerator : public NeighborGenerator { public: - MOCK_CONST_METHOD2(generate_neighbors, std::vector(cav_msgs::ManeuverPlan, const VehicleState&)); + MOCK_CONST_METHOD2(generate_neighbors, std::vector(carma_planning_msgs::msg::ManeuverPlan, const VehicleState&)); ~MockNeighborGenerator(){}; }; @@ -57,123 +58,127 @@ namespace arbitrator { public: TreePlannerTest(): - tp{mcf, mng, mss, ros::Duration(5)} {}; - MockSearchStrategy mss; - MockCostFunction mcf; - MockNeighborGenerator mng; + tp{mcf, + mng, + mss, + rclcpp::Duration(5, 0)} {}; + std::shared_ptr mss = std::shared_ptr(new MockSearchStrategy); + std::shared_ptr mcf = std::shared_ptr(new MockCostFunction); + std::shared_ptr mng = std::shared_ptr(new MockNeighborGenerator); TreePlanner tp; }; TEST_F(TreePlannerTest, testGeneratePlan1) { - cav_msgs::ManeuverPlan plan1; - cav_msgs::Maneuver mvr1, mvr2, mvr3; + carma_planning_msgs::msg::ManeuverPlan plan1; + carma_planning_msgs::msg::Maneuver mvr1, mvr2, mvr3; - mvr1.type = cav_msgs::Maneuver::LANE_FOLLOWING; - mvr1.lane_following_maneuver.start_time = ros::Time(0); - mvr1.lane_following_maneuver.end_time = ros::Time(5.0); + mvr1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + mvr1.lane_following_maneuver.start_time = rclcpp::Time(0, 0); + mvr1.lane_following_maneuver.end_time = rclcpp::Time(5, 0); plan1.maneuvers.push_back(mvr1); - std::vector plans{plan1}; + std::vector plans{plan1}; { InSequence seq; - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillOnce( Return(plans) ); - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillRepeatedly( - Return(std::vector()) + Return(std::vector()) ); } - EXPECT_CALL(mcf, compute_cost_per_unit_distance(_)) + + EXPECT_CALL(*mcf, compute_cost_per_unit_distance(_)) .WillRepeatedly( Return(5.0) ); - EXPECT_CALL(mss, prioritize_plans(_)) + EXPECT_CALL(*mss, prioritize_plans(_)) .WillRepeatedly( ReturnArg<0>() ); VehicleState state; - cav_msgs::ManeuverPlan plan = tp.generate_plan(state); + carma_planning_msgs::msg::ManeuverPlan plan = tp.generate_plan(state); ASSERT_FALSE(plan.maneuvers.empty()); ASSERT_EQ(1, plan.maneuvers.size()); - ASSERT_EQ(ros::Time(0), plan.maneuvers[0].lane_following_maneuver.start_time); - ASSERT_EQ(ros::Time(5), plan.maneuvers[0].lane_following_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(0, 0), rclcpp::Time(plan.maneuvers[0].lane_following_maneuver.start_time, RCL_SYSTEM_TIME)); + ASSERT_EQ(rclcpp::Time(5, 0), rclcpp::Time(plan.maneuvers[0].lane_following_maneuver.end_time, RCL_SYSTEM_TIME)); } TEST_F(TreePlannerTest, testGeneratePlan2) { - cav_msgs::ManeuverPlan plan1, plan2, plan3; - cav_msgs::Maneuver mvr1, mvr2, mvr3; + carma_planning_msgs::msg::ManeuverPlan plan1, plan2, plan3; + carma_planning_msgs::msg::Maneuver mvr1, mvr2, mvr3; - mvr1.type = cav_msgs::Maneuver::LANE_FOLLOWING; - mvr1.lane_following_maneuver.start_time = ros::Time(0); - mvr1.lane_following_maneuver.end_time = ros::Time(5.0); + mvr1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + mvr1.lane_following_maneuver.start_time = rclcpp::Time(0); + mvr1.lane_following_maneuver.end_time = rclcpp::Time(5, 0); - mvr2.type = cav_msgs::Maneuver::LANE_FOLLOWING; - mvr2.lane_following_maneuver.start_time = ros::Time(0); - mvr2.lane_following_maneuver.end_time = ros::Time(5.0); + mvr2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + mvr2.lane_following_maneuver.start_time = rclcpp::Time(0); + mvr2.lane_following_maneuver.end_time = rclcpp::Time(5, 0); - mvr3.type = cav_msgs::Maneuver::LANE_FOLLOWING; - mvr3.lane_following_maneuver.start_time = ros::Time(0); - mvr3.lane_following_maneuver.end_time = ros::Time(5.0); + mvr3.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + mvr3.lane_following_maneuver.start_time = rclcpp::Time(0); + mvr3.lane_following_maneuver.end_time = rclcpp::Time(5, 0); plan1.maneuvers.push_back(mvr1); plan2.maneuvers.push_back(mvr2); plan3.maneuvers.push_back(mvr3); - std::vector plans{plan1, plan2, plan3}; + std::vector plans{plan1, plan2, plan3}; { InSequence seq; - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillOnce( Return(plans) ); - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillRepeatedly( - Return(std::vector()) + Return(std::vector()) ); } - EXPECT_CALL(mcf, compute_cost_per_unit_distance(_)) + EXPECT_CALL(*mcf, compute_cost_per_unit_distance(_)) .WillRepeatedly( Return(5.0) ); - EXPECT_CALL(mss, prioritize_plans(_)) + EXPECT_CALL(*mss, prioritize_plans(_)) .WillRepeatedly( ReturnArg<0>() ); VehicleState state; - cav_msgs::ManeuverPlan plan = tp.generate_plan(state); + carma_planning_msgs::msg::ManeuverPlan plan = tp.generate_plan(state); ASSERT_FALSE(plan.maneuvers.empty()); ASSERT_EQ(1, plan.maneuvers.size()); - ASSERT_EQ(ros::Time(0), plan.maneuvers[0].lane_following_maneuver.start_time); - ASSERT_EQ(ros::Time(5), plan.maneuvers[0].lane_following_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(0, 0), rclcpp::Time(plan.maneuvers[0].lane_following_maneuver.start_time, RCL_SYSTEM_TIME)); + ASSERT_EQ(rclcpp::Time(5, 0), rclcpp::Time(plan.maneuvers[0].lane_following_maneuver.end_time, RCL_SYSTEM_TIME)); } TEST_F(TreePlannerTest, testGeneratePlan3) { - cav_msgs::ManeuverPlan plan1, plan2, plan3; - cav_msgs::Maneuver mvr1, mvr2, mvr3; + carma_planning_msgs::msg::ManeuverPlan plan1, plan2, plan3; + carma_planning_msgs::msg::Maneuver mvr1, mvr2, mvr3; - mvr1.type = cav_msgs::Maneuver::LANE_FOLLOWING; - mvr1.lane_following_maneuver.start_time = ros::Time(0); - mvr1.lane_following_maneuver.end_time = ros::Time(2); + mvr1.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + mvr1.lane_following_maneuver.start_time = rclcpp::Time(0); + mvr1.lane_following_maneuver.end_time = rclcpp::Time(2, 0); - mvr2.type = cav_msgs::Maneuver::LANE_FOLLOWING; - mvr2.lane_following_maneuver.start_time = ros::Time(2); - mvr2.lane_following_maneuver.end_time = ros::Time(4); + mvr2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + mvr2.lane_following_maneuver.start_time = rclcpp::Time(2, 0); + mvr2.lane_following_maneuver.end_time = rclcpp::Time(4, 0); - mvr3.type = cav_msgs::Maneuver::LANE_FOLLOWING; - mvr3.lane_following_maneuver.start_time = ros::Time(4); - mvr3.lane_following_maneuver.end_time = ros::Time(5); + mvr3.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + mvr3.lane_following_maneuver.start_time = rclcpp::Time(4, 0); + mvr3.lane_following_maneuver.end_time = rclcpp::Time(5, 0); plan1.maneuvers.push_back(mvr1); @@ -184,49 +189,49 @@ namespace arbitrator plan3.maneuvers.push_back(mvr2); plan3.maneuvers.push_back(mvr3); - std::vector plans1{plan1}; - std::vector plans2{plan2}; - std::vector plans3{plan3}; + std::vector plans1{plan1}; + std::vector plans2{plan2}; + std::vector plans3{plan3}; { InSequence seq; - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillOnce( Return(plans1) ); - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillOnce( Return(plans2) ); - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillOnce( Return(plans3) ); - EXPECT_CALL(mng, generate_neighbors(_,_)) + EXPECT_CALL(*mng, generate_neighbors(_,_)) .WillRepeatedly( - Return(std::vector()) + Return(std::vector()) ); } - EXPECT_CALL(mcf, compute_cost_per_unit_distance(_)) + EXPECT_CALL(*mcf, compute_cost_per_unit_distance(_)) .WillRepeatedly( Return(5.0) ); - EXPECT_CALL(mss, prioritize_plans(_)) + EXPECT_CALL(*mss, prioritize_plans(_)) .WillRepeatedly( ReturnArg<0>() ); VehicleState state; - cav_msgs::ManeuverPlan plan = tp.generate_plan(state); + carma_planning_msgs::msg::ManeuverPlan plan = tp.generate_plan(state); ASSERT_FALSE(plan.maneuvers.empty()); ASSERT_EQ(3, plan.maneuvers.size()); - ASSERT_EQ(ros::Time(0), plan.maneuvers[0].lane_following_maneuver.start_time); - ASSERT_EQ(ros::Time(2), plan.maneuvers[0].lane_following_maneuver.end_time); - ASSERT_EQ(ros::Time(2), plan.maneuvers[1].lane_following_maneuver.start_time); - ASSERT_EQ(ros::Time(4), plan.maneuvers[1].lane_following_maneuver.end_time); - ASSERT_EQ(ros::Time(4), plan.maneuvers[2].lane_following_maneuver.start_time); - ASSERT_EQ(ros::Time(5), plan.maneuvers[2].lane_following_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(0, 0), rclcpp::Time(plan.maneuvers[0].lane_following_maneuver.start_time, RCL_SYSTEM_TIME)); + ASSERT_EQ(rclcpp::Time(2, 0), rclcpp::Time(plan.maneuvers[0].lane_following_maneuver.end_time, RCL_SYSTEM_TIME)); + ASSERT_EQ(rclcpp::Time(2, 0), rclcpp::Time(plan.maneuvers[1].lane_following_maneuver.start_time, RCL_SYSTEM_TIME)); + ASSERT_EQ(rclcpp::Time(4, 0), rclcpp::Time(plan.maneuvers[1].lane_following_maneuver.end_time, RCL_SYSTEM_TIME)); + ASSERT_EQ(rclcpp::Time(4, 0), rclcpp::Time(plan.maneuvers[2].lane_following_maneuver.start_time, RCL_SYSTEM_TIME)); + ASSERT_EQ(rclcpp::Time(5, 0), rclcpp::Time(plan.maneuvers[2].lane_following_maneuver.end_time, RCL_SYSTEM_TIME)); } } \ No newline at end of file diff --git a/arbitrator/test/test_utils.h b/arbitrator/test/test_utils.h index 15513bb8bd..ca8d884d99 100644 --- a/arbitrator/test/test_utils.h +++ b/arbitrator/test/test_utils.h @@ -1,6 +1,6 @@ /*------------------------------------------------------------------------------ -* Copyright (C) 2019-2021 LEIDOS. +* 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 @@ -26,7 +26,6 @@ #define __ARBITRATOR_INCLUDE_TEST_UTILS_HPP__ #include -#include #include "arbitrator_state_machine.hpp" #include "fixed_priority_cost_function.hpp" #include "beam_search_strategy.hpp" diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 9a679f23b5..9fc013a174 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -86,9 +86,6 @@ - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index 6603d86776..b2277e2e15 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -64,6 +64,9 @@ def generate_launch_description(): guidance_param_file = os.path.join( get_package_share_directory('guidance'), 'config/parameters.yaml') + arbitrator_param_file_path = os.path.join( + get_package_share_directory('arbitrator'), 'config/arbitrator_params.yaml') + plan_delegator_param_file = os.path.join( get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml') @@ -164,6 +167,23 @@ def generate_launch_description(): vehicle_config_param_file ] ), + ComposableNode( + package='arbitrator', + plugin='arbitrator::ArbitratorNode', + name='arbitrator', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('arbitrator', env_log_levels) } + ], + remappings = [ + ("final_maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/arbitrator/final_maneuver_plan" ] ), + ("guidance_state", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state" ] ), + ], + parameters=[ + arbitrator_param_file_path, + vehicle_config_param_file + ] + ), ComposableNode( package='guidance', plugin='guidance::GuidanceWorker', From 8d0fb22c80bed09276adb98e1df449348221e4fe Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 30 Jun 2022 15:57:00 -0400 Subject: [PATCH 028/165] Feature/basic autonomy backport (#1838) * add __pycache to gitignore * backport ros2 Co-authored-by: Michael McConnell --- .../basic_autonomy_ros2/basic_autonomy.hpp | 7 +++++-- basic_autonomy_ros2/src/basic_autonomy.cpp | 21 ++++++++++++------- .../test/test_waypoint_generation.cpp | 2 +- 3 files changed, 20 insertions(+), 10 deletions(-) diff --git a/basic_autonomy_ros2/include/basic_autonomy_ros2/basic_autonomy.hpp b/basic_autonomy_ros2/include/basic_autonomy_ros2/basic_autonomy.hpp index 99a0607ec9..b89d9d5914 100644 --- a/basic_autonomy_ros2/include/basic_autonomy_ros2/basic_autonomy.hpp +++ b/basic_autonomy_ros2/include/basic_autonomy_ros2/basic_autonomy.hpp @@ -86,6 +86,7 @@ namespace basic_autonomy // computed curvature and output speeds double back_distance = 20; // Number of meters behind the first maneuver that need to be included in points for curvature calculation double buffer_ending_downtrack = 20.0; //The additional downtrack beyond requested end dist used to fit points along spline + std::string desired_controller_plugin = "default"; //The desired controller plugin for the generated trajectory }; @@ -148,12 +149,13 @@ namespace basic_autonomy * \param times The times which at the vehicle should arrive at the specified points. First point should have a value of 0. Units s * \param yaws The orientation the vehicle should achieve at each point. Units radians * \param startTime The absolute start time which will be used to update the input relative times. Units s + * \param desired_controller_plugin The name of the controller plugin for the generated trajectory. * * \return A list of trajectory points built from the provided inputs. */ std::vector trajectory_from_points_times_orientations( const std::vector &points, const std::vector ×, - const std::vector &yaws, rclcpp::Time startTime); + const std::vector &yaws, rclcpp::Time startTime, const std::string &desired_controller_plugin); /** * \brief Attaches back_distance length of points behind the future points @@ -339,7 +341,8 @@ namespace basic_autonomy int speed_moving_average_window_size, int curvature_moving_average_window_size, double back_distance, - double buffer_ending_downtrack); + double buffer_ending_downtrack, + std::string desired_controller_plugin = "default"); GeneralTrajConfig compose_general_trajectory_config(const std::string& trajectory_type, int default_downsample_ratio, diff --git a/basic_autonomy_ros2/src/basic_autonomy.cpp b/basic_autonomy_ros2/src/basic_autonomy.cpp index 1de2e43678..bc1f666af9 100644 --- a/basic_autonomy_ros2/src/basic_autonomy.cpp +++ b/basic_autonomy_ros2/src/basic_autonomy.cpp @@ -594,6 +594,8 @@ namespace basic_autonomy size_t time_boundary_exclusive_index = trajectory_utils::time_boundary_index(downtracks, speeds, time_span); + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "time_boundary_exclusive_index = " << time_boundary_exclusive_index); if (time_boundary_exclusive_index == 0) { @@ -702,7 +704,7 @@ namespace basic_autonomy std::vector trajectory_from_points_times_orientations( const std::vector &points, const std::vector ×, const std::vector &yaws, - rclcpp::Time startTime) + rclcpp::Time startTime, const std::string &desired_controller_plugin) { if (points.size() != times.size() || points.size() != yaws.size()) { @@ -720,8 +722,8 @@ namespace basic_autonomy tpp.x = points[i].x(); tpp.y = points[i].y(); tpp.yaw = yaws[i]; - - tpp.controller_plugin_name = "default"; + + tpp.controller_plugin_name = desired_controller_plugin; //tpp.planner_plugin_name //Planner plugin name is filled in the tactical plugin traj.push_back(tpp); @@ -796,6 +798,9 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "NearestPtIndex: " << nearest_pt_index); std::vector future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Ready to call constrain_to_time_boundary: future_points size = " << future_points.size() << ", trajectory_time_length = " << detailed_config.trajectory_time_length); + auto time_bound_points = constrain_to_time_boundary(future_points, detailed_config.trajectory_time_length); RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Got time_bound_points with size:" << time_bound_points.size()); @@ -915,7 +920,7 @@ namespace basic_autonomy std::vector yaw = {state.orientation, state.orientation}; //Keep current orientation std::vector traj_points = - trajectory_from_points_times_orientations(remaining_traj_points, times, yaw, state_time); + trajectory_from_points_times_orientations(remaining_traj_points, times, yaw, state_time, detailed_config.desired_controller_plugin); return traj_points; @@ -972,7 +977,7 @@ namespace basic_autonomy // Build trajectory points std::vector traj_points = - trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time); + trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, state_time, detailed_config.desired_controller_plugin); //debug msg carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds msg; @@ -1003,7 +1008,8 @@ namespace basic_autonomy int speed_moving_average_window_size, int curvature_moving_average_window_size, double back_distance, - double buffer_ending_downtrack) + double buffer_ending_downtrack, + std::string desired_controller_plugin) { DetailedTrajConfig detailed_config; @@ -1016,6 +1022,7 @@ namespace basic_autonomy detailed_config.curvature_moving_average_window_size = curvature_moving_average_window_size; detailed_config.back_distance = back_distance; detailed_config.buffer_ending_downtrack = buffer_ending_downtrack; + detailed_config.desired_controller_plugin = desired_controller_plugin; return detailed_config; } @@ -1102,7 +1109,7 @@ namespace basic_autonomy RCLCPP_DEBUG_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "After removing extra buffer points, future_geom_points.size():"<< future_geom_points.size()); std::vector traj_points = - trajectory_from_points_times_orientations(future_geom_points, times, final_yaw_values, state_time); + trajectory_from_points_times_orientations(future_geom_points, times, final_yaw_values, state_time, detailed_config.desired_controller_plugin); return traj_points; } diff --git a/basic_autonomy_ros2/test/test_waypoint_generation.cpp b/basic_autonomy_ros2/test/test_waypoint_generation.cpp index 674e717943..4d6b0b5686 100644 --- a/basic_autonomy_ros2/test/test_waypoint_generation.cpp +++ b/basic_autonomy_ros2/test/test_waypoint_generation.cpp @@ -82,7 +82,7 @@ namespace basic_autonomy std::vector yaws = {0.2, 0.5, 0.6, 1.0}; rclcpp::Time startTime(1.0 * 1e9); // Arbitrarily selected 1 second, converted to nanoseconds std::vector traj_points = - basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(points, times, yaws, startTime); + basic_autonomy::waypoint_generation::trajectory_from_points_times_orientations(points, times, yaws, startTime, "default"); ASSERT_EQ(4, traj_points.size()); ASSERT_NEAR(1.0, rclcpp::Time(traj_points[0].target_time).seconds(), 0.0000001); From bba6a4be32892ec624422e82ab6d9a783c779df4 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Tue, 5 Jul 2022 16:48:13 -0400 Subject: [PATCH 029/165] Feature/plugin controller (#1811) * WIP plugin base library * WIP plugin controller * Completed first iteration on plugin base classes * WIP making plugin template package * wip sortaworks for headers * revert * Working template package for ROS2 plugins * add world model and sonar scan file updates * fix build error * WIP * wip * WIP * Fix typos in readme * Modify plugin base class to use fully specified node name instead of user provided name in plugin_discovery * basic logic and structure completed * fixing build errors * buildable guidance_controller * handling todo * Add parameters to config * working on adding checks to prevent conflicts with ros1 * Address PR comments * fix * cleanup most but not all plugin names * Add remaining name fixes and remove unused plugins * remove all references to wz_strategic_plugin and unobstructed_lanechange * cleanup health monitor * fix missed names * Move all plugins to plugins namespace * topic remapping fixes * fix typo * fixes * fixes * fixes * fix ros2 check * Implement unit tests * Fill out list of plugins * Fix typos * cleanup plugin names (#1828) * cleanup most but not all plugin names * Add remaining name fixes and remove unused plugins * remove all references to wz_strategic_plugin and unobstructed_lanechange * fix missed names * Fix typos Co-authored-by: John Stark --- .sonarqube/sonar-scanner.properties | 8 - arbitrator/config/arbitrator_params.yaml | 10 +- carma/launch/guidance.launch | 186 +++--- .../plugin_base_node.hpp | 10 +- carma_guidance_plugins/src/control_plugin.cpp | 2 +- .../src/plugin_base_node.cpp | 2 +- .../src/strategic_plugin.cpp | 2 +- .../src/tactical_plugin.cpp | 2 +- carma_guidance_plugins/test/TestPlugins.h | 12 - carma_guidance_plugins/test/node_test.cpp | 6 +- carma_wm_ctrl/README.md | 6 +- .../src/cooperative_lanechange.cpp | 6 +- .../guidance_plugin_components.py | 16 +- health_monitor/CMakeLists.txt | 3 - health_monitor/config/params.yaml | 8 +- health_monitor/include/health_monitor.h | 20 - health_monitor/include/plugin_manager.h | 85 --- health_monitor/src/health_monitor.cpp | 44 -- health_monitor/src/plugin_manager.cpp | 126 ---- health_monitor/test/test_health_monitor.cpp | 1 - health_monitor/test/test_plugin_manager.cpp | 101 ---- .../inlanecruising_plugin_node.h | 4 +- inlanecruising_plugin/src/helper.cpp | 2 +- .../src/inlanecruising_plugin.cpp | 4 +- inlanecruising_plugin/test/rostest_ilc.cpp | 2 +- .../test/test_inlanecruising_plugin.cpp | 2 +- .../intersection_transit_maneuvering_node.h | 6 +- .../src/intersection_transit_maneuvering.cpp | 2 +- lci_strategic_plugin/config/parameters.yaml | 8 +- .../lci_strategic_plugin_config.h | 8 +- lci_strategic_plugin/src/main.cpp | 2 +- ...ight_controlled_intersection_plugin_node.h | 2 +- ...ontrolled_intersection_tactical_plugin.cpp | 2 +- platooning_control/src/platoon_control.cpp | 4 +- platooning_control/test/test_mynode.cpp | 2 +- .../src/platoon_control_ihp.cpp | 4 +- platooning_control_ihp/test/test_mynode.cpp | 2 +- platooning_strategic/include/platoon_config.h | 2 +- .../include/platoon_strategic_plugin_node.h | 4 +- .../src/platoon_strategic.cpp | 12 +- .../include/platoon_config_ihp.h | 2 +- .../platoon_strategic_plugin_node_ihp.h | 4 +- .../src/platoon_strategic_ihp.cpp | 12 +- .../platooning_tactical_plugin_config.h | 2 +- .../platooning_tactical_plugin_node.h | 2 +- .../src/platooning_tactical_plugin.cpp | 4 +- .../src/port_drayage_plugin.cpp | 4 +- .../src/pure_pursuit_wrapper.cpp | 4 +- .../test/pure_pursuit_wrapper_test.cpp | 4 +- route_following_plugin/config/parameters.yaml | 8 +- .../include/route_following_plugin.h | 8 +- .../src/route_following_plugin.cpp | 2 +- .../test/test_route_following_plugin.cpp | 2 +- sci_strategic_plugin/config/parameters.yaml | 8 +- .../include/sci_strategic_plugin_config.h | 8 +- sci_strategic_plugin/src/main.cpp | 2 +- .../include/stop_and_wait_config.h | 2 +- .../launch/stop_and_wait_plugin.launch | 2 +- .../src/stop_and_wait_plugin.cpp | 4 +- ...stop_controlled_intersection_plugin_node.h | 2 +- ...ontrolled_intersection_tactical_plugin.cpp | 2 +- subsystem_controllers/CMakeLists.txt | 10 +- .../config/guidance_controller_config.yaml | 26 +- .../base_subsystem_controller.hpp | 11 + .../guidance_controller/entry.h | 52 ++ .../guidance_controller/entry_manager.h | 72 +++ .../guidance_controller.hpp | 47 +- .../guidance_controller_config.hpp | 61 ++ .../guidance_controller/plugin_manager.h | 224 +++++++ subsystem_controllers/package.xml | 1 + .../base_subsystem_controller.cpp | 16 + .../src/guidance_controller/entry_manager.cpp | 67 +++ .../guidance_controller.cpp | 224 +++++++ .../guidance_controller/plugin_manager.cpp | 532 ++++++++++++++++ subsystem_controllers/test/CMakeLists.txt | 2 + .../test/test_entry_manager.cpp | 162 +++++ .../test/test_plugin_manager.cpp | 380 ++++++++++++ template_package/carma_package | 1 - .../template_control_plugin_node.hpp | 2 - .../template_strategic_plugin_node.hpp | 2 - .../template_tactical_plugin_node.hpp | 2 - .../src/template_control_plugin_node.cpp | 4 - .../src/template_strategic_plugin_node.cpp | 4 - .../src/template_tactical_plugin_node.cpp | 4 - trajectory_executor/config/parameters.yaml | 2 +- .../trajectory_executor_config.hpp | 2 +- .../src/trajectory_executor_node.cpp | 4 +- .../test/trajectory_executor_test_suite.cpp | 6 +- unobstructed_lanechange/CMakeLists.txt | 133 ---- .../config/parameters.yaml | 81 --- .../include/unobstructed_lanechange.h | 164 ----- .../launch/unobstructed_lanechange.launch | 27 - unobstructed_lanechange/package.xml | 39 -- unobstructed_lanechange/src/main.cpp | 26 - .../src/unobstructed_lanechange.cpp | 213 ------- unobstructed_lanechange/test/TestHelpers.h | 162 ----- unobstructed_lanechange/test/helper.cpp | 42 -- unobstructed_lanechange/test/rostest_ulc.cpp | 73 --- .../test_unobstructed_lanechange_plugin.cpp | 241 -------- unobstructed_lanechange/test/ulc_plugin.test | 25 - wz_strategic_plugin/CMakeLists.txt | 96 --- wz_strategic_plugin/config/parameters.yaml | 26 - .../wz_state_transition_table.h | 83 --- .../include/wz_strategic_plugin/wz_states.h | 51 -- .../wz_strategic_plugin/wz_strategic_plugin.h | 291 --------- .../wz_strategic_plugin_config.h | 58 -- .../launch/wz_strategic_plugin.launch | 20 - wz_strategic_plugin/package.xml | 32 - wz_strategic_plugin/src/main.cpp | 72 --- .../src/wz_state_transition_table.cpp | 143 ----- wz_strategic_plugin/src/wz_states.cpp | 50 -- .../src/wz_strategic_plugin.cpp | 566 ------------------ wz_strategic_plugin/test/test_fixture.h | 66 -- wz_strategic_plugin/test/test_main.cpp | 30 - .../test/test_strategic_plugin.cpp | 183 ------ .../test/test_strategic_plugin_helpers.cpp | 225 ------- .../test/test_transition_table.cpp | 122 ---- .../include/yield_plugin/yield_plugin_node.h | 2 +- yield_plugin/src/yield_plugin.cpp | 4 +- yield_plugin/test/test_yield_plugin.cpp | 2 +- 120 files changed, 2096 insertions(+), 3990 deletions(-) delete mode 100644 health_monitor/include/plugin_manager.h delete mode 100644 health_monitor/src/plugin_manager.cpp delete mode 100644 health_monitor/test/test_plugin_manager.cpp create mode 100644 subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h create mode 100644 subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h create mode 100644 subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp create mode 100644 subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h create mode 100644 subsystem_controllers/src/guidance_controller/entry_manager.cpp create mode 100644 subsystem_controllers/src/guidance_controller/plugin_manager.cpp create mode 100644 subsystem_controllers/test/test_entry_manager.cpp create mode 100644 subsystem_controllers/test/test_plugin_manager.cpp delete mode 100644 unobstructed_lanechange/CMakeLists.txt delete mode 100644 unobstructed_lanechange/config/parameters.yaml delete mode 100644 unobstructed_lanechange/include/unobstructed_lanechange.h delete mode 100644 unobstructed_lanechange/launch/unobstructed_lanechange.launch delete mode 100644 unobstructed_lanechange/package.xml delete mode 100644 unobstructed_lanechange/src/main.cpp delete mode 100644 unobstructed_lanechange/src/unobstructed_lanechange.cpp delete mode 100644 unobstructed_lanechange/test/TestHelpers.h delete mode 100644 unobstructed_lanechange/test/helper.cpp delete mode 100644 unobstructed_lanechange/test/rostest_ulc.cpp delete mode 100644 unobstructed_lanechange/test/test_unobstructed_lanechange_plugin.cpp delete mode 100644 unobstructed_lanechange/test/ulc_plugin.test delete mode 100644 wz_strategic_plugin/CMakeLists.txt delete mode 100644 wz_strategic_plugin/config/parameters.yaml delete mode 100644 wz_strategic_plugin/include/wz_strategic_plugin/wz_state_transition_table.h delete mode 100644 wz_strategic_plugin/include/wz_strategic_plugin/wz_states.h delete mode 100644 wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h delete mode 100644 wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin_config.h delete mode 100644 wz_strategic_plugin/launch/wz_strategic_plugin.launch delete mode 100644 wz_strategic_plugin/package.xml delete mode 100644 wz_strategic_plugin/src/main.cpp delete mode 100644 wz_strategic_plugin/src/wz_state_transition_table.cpp delete mode 100644 wz_strategic_plugin/src/wz_states.cpp delete mode 100644 wz_strategic_plugin/src/wz_strategic_plugin.cpp delete mode 100644 wz_strategic_plugin/test/test_fixture.h delete mode 100644 wz_strategic_plugin/test/test_main.cpp delete mode 100644 wz_strategic_plugin/test/test_strategic_plugin.cpp delete mode 100644 wz_strategic_plugin/test/test_strategic_plugin_helpers.cpp delete mode 100644 wz_strategic_plugin/test/test_transition_table.cpp diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index b6bc2bd1ee..3c5a8ffe48 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -58,12 +58,10 @@ sonar.modules= bsm_generator, \ carma_record, \ traffic_incident_parser, \ motion_computation, \ - unobstructed_lanechange, \ yield_plugin, \ basic_autonomy, \ cooperative_lanechange,\ mobilitypath_visualizer, \ - wz_strategic_plugin, \ sci_strategic_plugin, \ stop_controlled_intersection_tactical_plugin, \ lci_strategic_plugin, \ @@ -105,12 +103,10 @@ stop_and_wait_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/stop_an carma_record.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_record traffic_incident_parser.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/traffic_incident_parser motion_computation.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/motion_computation -unobstructed_lanechange.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/unobstructed_lanechange cooperative_lanechange.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/cooperative_lanechange yield_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/yield_plugin basic_autonomy.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/basic_autonomy mobilitypath_visualizer.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/mobilitypath_visualizer -wz_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/wz_strategic_plugin sci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/sci_strategic_plugin stop_controlled_intersection_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/stop_controlled_intersection_tactical_plugin system_controller.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/system_controller @@ -154,12 +150,10 @@ stop_and_wait_plugin.sonar.sources = src carma_record.sonar.sources = src traffic_incident_parser.sonar.sources = src motion_computation.sonar.sources = src -unobstructed_lanechange.sonar.sources = src cooperative_lanechange.sonar.sources = src yield_plugin.sonar.sources = src basic_autonomy.sonar.sources = src mobilitypath_visualizer.sonar.sources = src -wz_strategic_plugin.sonar.sources = src sci_strategic_plugin.sonar.sources = src stop_controlled_intersection_tactical_plugin.sonar.sources = src system_controller.sonar.sources = src @@ -200,12 +194,10 @@ inlanecruising_plugin.sonar.tests = test stop_and_wait_plugin.sonar.tests = test traffic_incident_parser.sonar.tests = test motion_computation.sonar.tests = test -unobstructed_lanechange.sonar.tests = test cooperative_lanechange.sonar.tests = test yield_plugin.sonar.tests = test basic_autonomy.sonar.tests = test mobilitypath_visualizer.sonar.tests = test -wz_strategic_plugin.sonar.tests = test sci_strategic_plugin.sonar.tests = test platoon_control_ihp.sonar.tests = test diff --git a/arbitrator/config/arbitrator_params.yaml b/arbitrator/config/arbitrator_params.yaml index 2590564a26..8bd04b2624 100644 --- a/arbitrator/config/arbitrator_params.yaml +++ b/arbitrator/config/arbitrator_params.yaml @@ -32,11 +32,11 @@ plugin_priorities: ' { "plugin_priorities": { - "PlatooningStrategicIHPPlugin": 4.0, - "LCIStrategicPlugin": 3.0, - "SCIStrategicPlugin": 2.0, - "PlatooningStrategicPlugin": 2.0, - "RouteFollowingPlugin": 1.0 + "platoon_strategic_ihp": 4.0, + "lci_strategic_plugin": 3.0, + "sci_strategic_plugin": 2.0, + "platoon_strategic": 2.0, + "route_following_plugin": 1.0 } } ' diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 9fc013a174..645ef6be74 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -79,7 +79,7 @@ - + @@ -89,26 +89,6 @@ - - - - - - - - - - - - - - - - - - - @@ -139,34 +119,101 @@ - - - - - - + - - - - - - - - - - + + + + + - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - @@ -174,12 +221,6 @@ - - - - - - @@ -187,46 +228,7 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + diff --git a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp index 72dac6ce1d..da216637bf 100644 --- a/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp +++ b/carma_guidance_plugins/include/carma_guidance_plugins/plugin_base_node.hpp @@ -32,7 +32,7 @@ namespace carma_guidance_plugins * This includes basic state machine management (largely delegated to ROS2 lifecycle behavior), required interfaces, and plugin discovery * * Extending classes must implement the on_configure_plugin method to load parameters, and my override the other state transitions methods on__plugin if desired. - * Additionally, extending classes must implement the methods such as get_plugin_name() which are used to populate the plugin discovery message. + * Additionally, extending classes must implement the methods such as get_version_id() which are used to populate the plugin discovery message. * * NOTE: At the moment, this class and all extending classes are setup to support only single threading. * @@ -132,14 +132,6 @@ namespace carma_guidance_plugins */ virtual std::string get_capability() = 0; - /** - * \brief Returns the name of this plugin. - * This name may be automatically prepended to plugin API service or topic names. - * - * \return Name of this plugin - */ - virtual std::string get_plugin_name() = 0; - /** * \brief Returns the version id of this plugin. * diff --git a/carma_guidance_plugins/src/control_plugin.cpp b/carma_guidance_plugins/src/control_plugin.cpp index 401a748be6..0d6dd7f8d4 100644 --- a/carma_guidance_plugins/src/control_plugin.cpp +++ b/carma_guidance_plugins/src/control_plugin.cpp @@ -63,7 +63,7 @@ namespace carma_guidance_plugins current_velocity_sub_ = create_subscription("vehicle/twist", 1, std::bind(&ControlPlugin::current_twist_callback, this, std_ph::_1)); - trajectory_plan_sub_ = create_subscription(get_plugin_name() + "/plan_trajectory", 1, + trajectory_plan_sub_ = create_subscription(std::string(get_name()) + "/plan_trajectory", 1, std::bind(&ControlPlugin::current_trajectory_callback, this, std_ph::_1)); vehicle_cmd_pub_ = create_publisher("ctrl_raw", 1); diff --git a/carma_guidance_plugins/src/plugin_base_node.cpp b/carma_guidance_plugins/src/plugin_base_node.cpp index 358a3ed835..22f171df7d 100644 --- a/carma_guidance_plugins/src/plugin_base_node.cpp +++ b/carma_guidance_plugins/src/plugin_base_node.cpp @@ -142,7 +142,7 @@ namespace carma_guidance_plugins } carma_planning_msgs::msg::Plugin msg; - msg.name = get_plugin_name(); + msg.name = std::string(get_namespace()) + std::string(get_name()); msg.version_id = get_version_id(); msg.type = get_type(); msg.available = get_availability(); diff --git a/carma_guidance_plugins/src/strategic_plugin.cpp b/carma_guidance_plugins/src/strategic_plugin.cpp index 9eb2868af8..1d8016bfa4 100644 --- a/carma_guidance_plugins/src/strategic_plugin.cpp +++ b/carma_guidance_plugins/src/strategic_plugin.cpp @@ -38,7 +38,7 @@ namespace carma_guidance_plugins carma_ros2_utils::CallbackReturn StrategicPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { // Initialize plan maneuvers service - plan_maneuvers_service_ = create_service(get_plugin_name() + "/plan_maneuvers", + plan_maneuvers_service_ = create_service(std::string(get_name()) + "/plan_maneuvers", [this] (auto header, auto req, auto resp) { if (this->get_activation_status()) // Only trigger when activated { diff --git a/carma_guidance_plugins/src/tactical_plugin.cpp b/carma_guidance_plugins/src/tactical_plugin.cpp index 024e3250e6..94fe88737b 100644 --- a/carma_guidance_plugins/src/tactical_plugin.cpp +++ b/carma_guidance_plugins/src/tactical_plugin.cpp @@ -38,7 +38,7 @@ namespace carma_guidance_plugins carma_ros2_utils::CallbackReturn TacticalPlugin::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { // Initialize plan trajectory service - plan_trajectory_service_ = create_service(get_plugin_name() + "/plan_trajectory", + plan_trajectory_service_ = create_service(std::string(get_name()) + "/plan_trajectory", [this] (auto header, auto req, auto resp) { if (this->get_activation_status()) // Only trigger when activated { diff --git a/carma_guidance_plugins/test/TestPlugins.h b/carma_guidance_plugins/test/TestPlugins.h index 5a8b0daf29..6a35cfaea5 100644 --- a/carma_guidance_plugins/test/TestPlugins.h +++ b/carma_guidance_plugins/test/TestPlugins.h @@ -43,10 +43,6 @@ namespace carma_guidance_plugins return true; } - std::string get_plugin_name() override { - return "TestStrategicPlugin"; - } - std::string get_version_id() override { return "1.0"; } @@ -84,10 +80,6 @@ namespace carma_guidance_plugins return true; } - std::string get_plugin_name() override { - return "TestTacticalPlugin"; - } - std::string get_version_id() override { return "1.1"; } @@ -123,10 +115,6 @@ namespace carma_guidance_plugins return true; } - std::string get_plugin_name() override { - return "TestControlPlugin"; - } - std::string get_version_id() override { return "1.2"; } diff --git a/carma_guidance_plugins/test/node_test.cpp b/carma_guidance_plugins/test/node_test.cpp index 0beb9e7659..7507f09c7e 100644 --- a/carma_guidance_plugins/test/node_test.cpp +++ b/carma_guidance_plugins/test/node_test.cpp @@ -95,9 +95,9 @@ TEST(carma_guidance_plugins_test, connections_test) { ASSERT_TRUE(tactical_plugin->get_availability()); ASSERT_TRUE(control_plugin->get_availability()); - ASSERT_EQ(strategic_plugin->get_plugin_name(), "TestStrategicPlugin"); - ASSERT_EQ(tactical_plugin->get_plugin_name(), "TestTacticalPlugin"); - ASSERT_EQ(control_plugin->get_plugin_name(), "TestControlPlugin"); + ASSERT_EQ(strategic_plugin->get_name(), "strategic_plugin_test"); + ASSERT_EQ(tactical_plugin->get_name(), "tactical_plugin_test"); + ASSERT_EQ(control_plugin->get_name(), "control_plugin_test"); ASSERT_EQ(strategic_plugin->get_version_id(), "1.0"); ASSERT_EQ(tactical_plugin->get_version_id(), "1.1"); diff --git a/carma_wm_ctrl/README.md b/carma_wm_ctrl/README.md index 230a07eea4..1db3c2a183 100644 --- a/carma_wm_ctrl/README.md +++ b/carma_wm_ctrl/README.md @@ -1,5 +1,7 @@ # carma_wm_ctrl This packages provides logic for updating a [carma_wm](../carma_wm) compatable lanelet2 map at runtime using [CARMACloud](https://github.com/usdot-fhwa-stol/carma-cloud) geofences. -The carma_wm_broadcaster node sits between the lanelet2 map loader and the rest of the CARMAPlatform system. When a new geofence is recieved, the map is updated and the update is communicated to the rest of the CARMAPlatform. -If a base map is recieved which does not contain the carma_wm compatable regulatory elements, the carma_wm_broadcaster node will make an initial best effort attempt to make the map compatable. There is no guarenetee that this will work so starting with a compatible map is always recommended. +The carma_wm_broadcaster node sits between the lanelet2 map loader and the rest of the CARMAPlatform system. When a new geofence is received, the map is updated and the update is communicated to the rest of the CARMAPlatform. +If a base map is received which does not contain the carma_wm compatible regulatory elements, the carma_wm_broadcaster node will make an initial best effort attempt to make the map compatible. There is no guarantee that this will work so starting with a compatible map is always recommended. + +Design documentation for this package can be found in the carma_wm_ctrl section of [this confluence page.](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/1324122268/Detailed+Design+-+World+Model+Road+Network+Library) diff --git a/cooperative_lanechange/src/cooperative_lanechange.cpp b/cooperative_lanechange/src/cooperative_lanechange.cpp index 7de53b2011..9c081f5b03 100644 --- a/cooperative_lanechange/src/cooperative_lanechange.cpp +++ b/cooperative_lanechange/src/cooperative_lanechange.cpp @@ -44,13 +44,13 @@ namespace cooperative_lanechange nh_.reset(new ros::CARMANodeHandle()); pnh_.reset(new ros::CARMANodeHandle("~")); - trajectory_srv_ = nh_->advertiseService("plugins/CooperativeLaneChangePlugin/plan_trajectory", &CooperativeLaneChangePlugin::plan_trajectory_cb, this); + trajectory_srv_ = nh_->advertiseService("cooperative_lanechange/plan_trajectory", &CooperativeLaneChangePlugin::plan_trajectory_cb, this); cooperative_lanechange_plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); - plugin_discovery_msg_.name = "CooperativeLaneChangePlugin"; + plugin_discovery_msg_.name = "cooperative_lanechange"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; diff --git a/guidance_plugin_validator/src/guidance_plugin_validator/guidance_plugin_components.py b/guidance_plugin_validator/src/guidance_plugin_validator/guidance_plugin_components.py index b24726cfd9..f1ebd5600c 100644 --- a/guidance_plugin_validator/src/guidance_plugin_validator/guidance_plugin_components.py +++ b/guidance_plugin_validator/src/guidance_plugin_validator/guidance_plugin_components.py @@ -31,7 +31,7 @@ def __init__(self, plugin_name): self.plugin_name = plugin_name # Set the plugin's node name - if plugin_name == "RouteFollowing": + if plugin_name == "route_following_plugin": self.node_name = "/guidance/route_following_plugin" else: rospy.logerr("ERROR: Unknown node for Strategic Plugin: " + str(self.plugin_name)) @@ -158,15 +158,13 @@ def __init__(self, plugin_name): self.plugin_name = plugin_name # Set the plugin's node name - if plugin_name == "InLaneCruisingPlugin": + if plugin_name == "inlanecruising_plugin": self.node_name = "/guidance/inlanecruising_plugin" - elif plugin_name == "StopandWaitPlugin": + elif plugin_name == "stop_and_wait_plugin": self.node_name = "/guidance/stop_and_wait_plugin" - elif plugin_name == "CooperativeLaneChangePlugin": + elif plugin_name == "cooperative_lanechange": self.node_name = "/guidance/cooperative_lanechange" - elif plugin_name == "UnobstructedLaneChangePlugin": - self.node_name = "/guidance/unobstructed_lanechange" - elif plugin_name == "YieldPlugin": + elif plugin_name == "yield_plugin": self.node_name = "/guidance/yield_plugin" else: rospy.logerr("ERROR: Unknown node for Tactical Plugin: " + str(self.plugin_name)) @@ -293,9 +291,9 @@ def __init__(self, plugin_name): self.plugin_name = plugin_name # Set the plugin's node name and plugin_discovery capability - if plugin_name == "Pure Pursuit": + if plugin_name == "pure_pursuit_wrapper_node": self.node_name = "/guidance/pure_pursuit_wrapper_node" - self.capability = "control_pure_pursuit_plan/plan_controls" + self.capability = "control/trajectory_control" self.plan_trajectory_topic = "/guidance/pure_pursuit/plan_trajectory" else: rospy.logerr("ERROR: Unknown node for Control Plugin: " + str(self.plugin_name)) diff --git a/health_monitor/CMakeLists.txt b/health_monitor/CMakeLists.txt index b79062189b..040fb50f87 100644 --- a/health_monitor/CMakeLists.txt +++ b/health_monitor/CMakeLists.txt @@ -62,7 +62,6 @@ include_directories( ## The recommended prefix ensures that target names across packages don't collide add_executable(${PROJECT_NAME} src/health_monitor.cpp - src/plugin_manager.cpp src/driver_manager.cpp src/entry_manager.cpp src/entry.cpp @@ -71,7 +70,6 @@ add_executable(${PROJECT_NAME} ## Add cmake target dependencies of the executable add_library(plugin_driver_manager_library - src/plugin_manager.cpp src/driver_manager.cpp src/entry_manager.cpp src/health_monitor.cpp @@ -124,7 +122,6 @@ install(DIRECTORY catkin_add_gtest(${PROJECT_NAME}-test test/test_driver_manager.cpp test/test_entry_manager.cpp - test/test_plugin_manager.cpp test/test_health_monitor.cpp test/test_main.cpp) # if(TARGET ${PROJECT_NAME}-test) diff --git a/health_monitor/config/params.yaml b/health_monitor/config/params.yaml index aeec4ba4a4..168e9121f5 100644 --- a/health_monitor/config/params.yaml +++ b/health_monitor/config/params.yaml @@ -26,10 +26,10 @@ startup_duration: 30.0 # List of String: Required plugins for the platform to be functional required_plugins: - - RouteFollowing - - InLaneCruisingPlugin - - StopAndWaitPlugin - - Pure Pursuit + - route_following_plugin + - inlanecruising_plugin + - stop_and_wait_plugin + - pure_pursuit_wrapper_node # String: Plugin service name prefix plugin_service_prefix: /guidance/plugins/ diff --git a/health_monitor/include/health_monitor.h b/health_monitor/include/health_monitor.h index b3a4e22d69..79d3f64511 100644 --- a/health_monitor/include/health_monitor.h +++ b/health_monitor/include/health_monitor.h @@ -22,7 +22,6 @@ #include #include #include -#include "plugin_manager.h" #include "driver_manager.h" namespace health_monitor @@ -59,25 +58,12 @@ namespace health_monitor std::shared_ptr pnh_; // workers - PluginManager plugin_manager_; DriverManager driver_manager_; - // service servers - ros::ServiceServer registered_plugin_service_server_; - ros::ServiceServer active_plugin_service_server_; - ros::ServiceServer activate_plugin_service_server_; - ros::ServiceServer get_strategic_plugin_by_capability_server_; - ros::ServiceServer get_tactical_plugin_by_capability_server_; - // topic subscribers - ros::Subscriber plugin_discovery_subscriber_; ros::Subscriber driver_discovery_subscriber_; // message/service callbacks - bool registered_plugin_cb(cav_srvs::PluginListRequest& req, cav_srvs::PluginListResponse& res); - bool active_plugin_cb(cav_srvs::PluginListRequest& req, cav_srvs::PluginListResponse& res); - bool activate_plugin_cb(cav_srvs::PluginActivationRequest& req, cav_srvs::PluginActivationResponse& res); - void plugin_discovery_cb(const cav_msgs::PluginConstPtr& msg); void driver_discovery_cb(const cav_msgs::DriverStatusConstPtr& msg); // initialize method @@ -89,18 +75,12 @@ namespace health_monitor std::vector lidar_gps_drivers_; std::vector camera_drivers_; - std::vector required_plugins_; bool truck_; bool car_; // record of startup timestamp long start_up_timestamp_; - // service name prefix and suffix - std::string plugin_service_prefix_; - std::string strategic_plugin_service_suffix_; - std::string tactical_plugin_service_suffix_; - // Previously published alert message boost::optional prev_alert; }; diff --git a/health_monitor/include/plugin_manager.h b/health_monitor/include/plugin_manager.h deleted file mode 100644 index bbd852d216..0000000000 --- a/health_monitor/include/plugin_manager.h +++ /dev/null @@ -1,85 +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 "entry_manager.h" -#include -#include - - -namespace health_monitor -{ - class PluginManager - { - public: - - /** - * \brief Default constructor for PluginManager - */ - PluginManager() = default; - - /** - * \brief Constructor for PluginManager takes in require_plugin_names and service names - */ - PluginManager(const std::vector& require_plugin_names, - const std::string& service_prefix, - const std::string& strategic_service_suffix, - const std::string& tactical_service_suffix); - - /** - * \brief Get a list of registered plugins - */ - void get_registered_plugins(cav_srvs::PluginListResponse& res); - - /** - * \brief Get a list of active plugins - */ - void get_active_plugins(cav_srvs::PluginListResponse& res); - - /** - * \brief Activate or deactivate a certain plugin - */ - bool activate_plugin(const std::string& name, const bool activate); - - /** - * \brief Update the status of a certain plugin - */ - void update_plugin_status(const cav_msgs::PluginConstPtr& msg); - - /** - * \brief Get strategic plugins by capability - */ - bool get_strategic_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res); - - /** - * \brief Get tactical plugins by capability - */ - bool get_tactical_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res); - - private: - - std::string service_prefix_; - std::string strategic_service_suffix_; - std::string tactical_service_suffix_; - EntryManager em_; - - }; -} \ No newline at end of file diff --git a/health_monitor/src/health_monitor.cpp b/health_monitor/src/health_monitor.cpp index 77da9aedf6..37ad46debd 100644 --- a/health_monitor/src/health_monitor.cpp +++ b/health_monitor/src/health_monitor.cpp @@ -34,23 +34,13 @@ namespace health_monitor pnh_.reset(new ros::CARMANodeHandle("~")); nh_.reset(new ros::CARMANodeHandle()); // init ros service servers, publishers and subscribers - registered_plugin_service_server_ = nh_->advertiseService("plugins/get_registered_plugins", &HealthMonitor::registered_plugin_cb, this); - active_plugin_service_server_ = nh_->advertiseService("plugins/get_active_plugins", &HealthMonitor::active_plugin_cb, this); - activate_plugin_service_server_ = nh_->advertiseService("plugins/activate_plugin", &HealthMonitor::activate_plugin_cb, this); - get_strategic_plugin_by_capability_server_ = nh_->advertiseService("plugins/get_strategic_plugin_by_capability", &PluginManager::get_strategic_plugins_by_capability, &plugin_manager_); - get_tactical_plugin_by_capability_server_ = nh_->advertiseService("plugins/get_tactical_plugin_by_capability", &PluginManager::get_tactical_plugins_by_capability, &plugin_manager_); - plugin_discovery_subscriber_ = nh_->subscribe("plugin_discovery", 10, &HealthMonitor::plugin_discovery_cb, this); 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); - plugin_service_prefix_ = pnh_->param("plugin_service_prefix", ""); - strategic_plugin_service_suffix_ = pnh_->param("strategic_plugin_service_suffix", ""); - tactical_plugin_service_suffix_ = pnh_->param("tactical_plugin_service_suffix", ""); - pnh_->getParam("required_plugins", required_plugins_); pnh_->getParam("required_drivers", required_drivers_); pnh_->getParam("lidar_gps_drivers", lidar_gps_drivers_); pnh_->getParam("camera_drivers",camera_drivers_); @@ -65,16 +55,8 @@ namespace health_monitor 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("plugin_service_prefix: " << plugin_service_prefix_); - ROS_INFO_STREAM("strategic_plugin_service_suffix: " << strategic_plugin_service_suffix_); - ROS_INFO_STREAM("tactical_plugin_service_suffix: " << tactical_plugin_service_suffix_); ROS_INFO_STREAM("truck: " << truck_); ROS_INFO_STREAM("car: " << car_); - ROS_INFO_STREAM("required_plugins: ["); - for(auto p : required_plugins_) { - ROS_INFO_STREAM(" " << p); - } - ROS_INFO_STREAM(" ]"); ROS_INFO_STREAM("required_drivers: ["); for(auto p : required_drivers_) { @@ -99,7 +81,6 @@ namespace health_monitor // initialize worker class - plugin_manager_ = PluginManager(required_plugins_, plugin_service_prefix_, strategic_plugin_service_suffix_, tactical_plugin_service_suffix_); driver_manager_ = DriverManager(required_drivers_, driver_timeout_,lidar_gps_drivers_,camera_drivers_); // record starup time @@ -164,31 +145,6 @@ namespace health_monitor ros::CARMANodeHandle::spin(); } - bool HealthMonitor::registered_plugin_cb(cav_srvs::PluginListRequest& req, cav_srvs::PluginListResponse& res) - { - plugin_manager_.get_registered_plugins(res); - return true; - } - - bool HealthMonitor::active_plugin_cb(cav_srvs::PluginListRequest& req, cav_srvs::PluginListResponse& res) - { - plugin_manager_.get_active_plugins(res); - return true; - } - - bool HealthMonitor::activate_plugin_cb(cav_srvs::PluginActivationRequest& req, cav_srvs::PluginActivationResponse& res) - { - bool answer = plugin_manager_.activate_plugin(req.plugin_name, req.activated); - if(answer) { - res.newstate = req.activated; - } - return answer; - } - - void HealthMonitor::plugin_discovery_cb(const cav_msgs::PluginConstPtr& msg) - { - plugin_manager_.update_plugin_status(msg); - } void HealthMonitor::driver_discovery_cb(const cav_msgs::DriverStatusConstPtr& msg) { diff --git a/health_monitor/src/plugin_manager.cpp b/health_monitor/src/plugin_manager.cpp deleted file mode 100644 index 5b50b9f228..0000000000 --- a/health_monitor/src/plugin_manager.cpp +++ /dev/null @@ -1,126 +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 "plugin_manager.h" - -namespace health_monitor -{ - - PluginManager::PluginManager(const std::vector& require_plugin_names, - const std::string& service_prefix, - const std::string& strategic_service_suffix, - const std::string& tactical_service_suffix) - : service_prefix_(service_prefix), - strategic_service_suffix_(strategic_service_suffix), - tactical_service_suffix_(tactical_service_suffix), - em_(EntryManager(require_plugin_names)) - {} - - void PluginManager::get_registered_plugins(cav_srvs::PluginListResponse& res) - { - std::vector plugins = em_.get_entries(); - // convert to plugin list - for(auto i = plugins.begin(); i < plugins.end(); ++i) - { - cav_msgs::Plugin plugin; - plugin.activated = i->active_; - plugin.available = i->available_; - plugin.name = i->name_; - plugin.type = i->type_; - res.plugins.push_back(plugin); - } - } - - void PluginManager::get_active_plugins(cav_srvs::PluginListResponse& res) - { - std::vector plugins = em_.get_entries(); - // convert to plugin list - for(auto i = plugins.begin(); i < plugins.end(); ++i) - { - if(i->active_) - { - cav_msgs::Plugin plugin; - plugin.activated = true; - plugin.available = i->available_; - plugin.name = i->name_; - plugin.type = i->type_; - res.plugins.push_back(plugin); - } - } - } - - bool PluginManager::activate_plugin(const std::string& name, const bool activate) - { - boost::optional requested_plugin = em_.get_entry_by_name(name); - if(requested_plugin) - { - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry updated_entry(requested_plugin->available_, activate, requested_plugin->name_, 0, requested_plugin->type_, requested_plugin->capability_); - em_.update_entry(updated_entry); - return true; - } - return false; - } - - void PluginManager::update_plugin_status(const cav_msgs::PluginConstPtr& msg) - { - ROS_DEBUG_STREAM("received status from: " << msg->name); - boost::optional requested_plugin = em_.get_entry_by_name(msg->name); - // params: bool available, bool active, std::string name, long timestamp, uint8_t type - Entry plugin(msg->available, msg->activated, msg->name, 0, msg->type, msg->capability); - // if it already exists, we do not change its activation status - if(requested_plugin) - { - plugin.active_ = requested_plugin->active_; - } else if(em_.is_entry_required(msg->name)) - { - plugin.active_ = true; - } - em_.update_entry(plugin); - } - - bool PluginManager::get_tactical_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res) - { - for(const auto& plugin : em_.get_entries()) - { - if(plugin.type_ == cav_msgs::Plugin::TACTICAL && - (req.capability.size() == 0 || (plugin.capability_.compare(0, req.capability.size(), req.capability) == 0 && plugin.active_ && plugin.available_))) - { - res.plan_service.push_back(service_prefix_ + plugin.name_ + tactical_service_suffix_); - } - } - return true; - } - - bool PluginManager::get_strategic_plugins_by_capability(cav_srvs::GetPluginApiRequest& req, cav_srvs::GetPluginApiResponse& res) - { - for(const auto& plugin : em_.get_entries()) - { - if(plugin.type_ == cav_msgs::Plugin::STRATEGIC && - (req.capability.size() == 0 || (plugin.capability_.compare(0, req.capability.size(), req.capability) == 0 && plugin.active_ && plugin.available_))) - { - ROS_DEBUG_STREAM("discovered strategic plugin: " << plugin.name_); - res.plan_service.push_back(service_prefix_ + plugin.name_ + strategic_service_suffix_); - } - else - { - ROS_DEBUG_STREAM("not valid strategic plugin: " << plugin.name_); - } - } - return true; - } - -} diff --git a/health_monitor/test/test_health_monitor.cpp b/health_monitor/test/test_health_monitor.cpp index 43bd2bd4b8..07e01a86a7 100644 --- a/health_monitor/test/test_health_monitor.cpp +++ b/health_monitor/test/test_health_monitor.cpp @@ -1,7 +1,6 @@ #include "health_monitor.h" #include "driver_manager.h" #include -#include "plugin_manager.h" #include #include diff --git a/health_monitor/test/test_plugin_manager.cpp b/health_monitor/test/test_plugin_manager.cpp deleted file mode 100644 index dc1db60939..0000000000 --- a/health_monitor/test/test_plugin_manager.cpp +++ /dev/null @@ -1,101 +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 "plugin_manager.h" -#include - -namespace health_monitor -{ - TEST(PluginManagerTest, testRegisteredPlugins) - { - std::vector required_plugins{"autoware", "pure_pursuit"}; - PluginManager pm(required_plugins, "/guidance/plugin/", "/plan_maneuver", "/plan_trajectory"); - cav_msgs::Plugin msg1; - msg1.name = "autoware"; - msg1.available = true; - msg1.activated = false; - msg1.type = cav_msgs::Plugin::STRATEGIC; - msg1.version_id = "1.0.1"; - msg1.capability = "waypoint_following/autoware"; - cav_msgs::PluginConstPtr msg1_pointer(new cav_msgs::Plugin(msg1)); - pm.update_plugin_status(msg1_pointer); - cav_msgs::Plugin msg2; - msg2.name = "lane_change"; - msg2.available = true; - msg2.activated = true; - // this field is not used by plugin manager - msg2.type = cav_msgs::Plugin::TACTICAL; - msg2.version_id = "1.0.0"; - msg2.capability = "lane_change"; - cav_msgs::PluginConstPtr msg2_pointer(new cav_msgs::Plugin(msg2)); - pm.update_plugin_status(msg2_pointer); - cav_srvs::PluginListResponse res; - pm.get_registered_plugins(res); - EXPECT_EQ(2, res.plugins.size()); - // for cruising plugin - EXPECT_EQ(true, res.plugins.begin()->activated); - EXPECT_EQ(true, res.plugins.begin()->available); - EXPECT_EQ(0, res.plugins.begin()->name.compare("autoware")); - //EXPECT_EQ(0, res.plugins.begin()->type); - //EXPECT_EQ(0, res.plugins.begin()->version_id.compare("")); - // for lane change plugin - EXPECT_EQ(false, std::prev(res.plugins.end())->activated); - EXPECT_EQ(true, std::prev(res.plugins.end())->available); - EXPECT_EQ(0, std::prev(res.plugins.end())->name.compare("lane_change")); - //EXPECT_EQ(0, std::prev(res.plugins.end())->type); - //EXPECT_EQ(0, std::prev(res.plugins.end())->version_id.compare("")); - // TODO need to handle type and verionID - cav_srvs::PluginListResponse res2; - pm.get_active_plugins(res2); - EXPECT_EQ(true, res2.plugins.begin()->activated); - EXPECT_EQ(true, res2.plugins.begin()->available); - EXPECT_EQ(0, res2.plugins.begin()->name.compare("autoware")); - pm.activate_plugin("lane_change", true); - cav_srvs::PluginListResponse res3; - pm.get_active_plugins(res3); - EXPECT_EQ(2, res3.plugins.size()); - EXPECT_EQ(true, std::prev(res3.plugins.end())->activated); - EXPECT_EQ(true, std::prev(res3.plugins.end())->available); - EXPECT_EQ(0, std::prev(res3.plugins.end())->name.compare("lane_change")); - pm.activate_plugin("lane_change", false); - cav_srvs::PluginListResponse res4; - pm.get_active_plugins(res4); - EXPECT_EQ(1, res4.plugins.size()); - EXPECT_EQ(true, res4.plugins.begin()->activated); - EXPECT_EQ(true, res4.plugins.begin()->available); - EXPECT_EQ(0, res4.plugins.begin()->name.compare("autoware")); - cav_srvs::GetPluginApiRequest req; - req.capability = ""; - cav_srvs::GetPluginApiResponse resp; - EXPECT_TRUE(pm.get_strategic_plugins_by_capability(req, resp)); - EXPECT_EQ(1, resp.plan_service.size()); - EXPECT_EQ("/guidance/plugin/autoware/plan_maneuver", resp.plan_service[0]); - resp = cav_srvs::GetPluginApiResponse(); - EXPECT_TRUE(pm.get_tactical_plugins_by_capability(req, resp)); - EXPECT_EQ(1, resp.plan_service.size()); - EXPECT_EQ("/guidance/plugin/lane_change/plan_trajectory", resp.plan_service[0]); - resp = cav_srvs::GetPluginApiResponse(); - req.capability = "waypoint_following"; - EXPECT_TRUE(pm.get_strategic_plugins_by_capability(req, resp)); - EXPECT_EQ(1, resp.plan_service.size()); - EXPECT_EQ("/guidance/plugin/autoware/plan_maneuver", resp.plan_service[0]); - resp = cav_srvs::GetPluginApiResponse(); - req.capability = "platooning"; - EXPECT_TRUE(pm.get_strategic_plugins_by_capability(req, resp)); - EXPECT_EQ(0, resp.plan_service.size()); - } - -} diff --git a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h index 9a0c6173bc..d18e7917b9 100644 --- a/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h +++ b/inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h @@ -84,11 +84,11 @@ class InLaneCruisingPluginNode InLaneCruisingPlugin worker(wm_, config, [&discovery_pub](const auto& msg) { discovery_pub.publish(msg); }, [&trajectory_debug_pub](const auto& msg) { trajectory_debug_pub.publish(msg); }); - ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/InLaneCruisingPlugin/plan_trajectory", + ros::ServiceServer trajectory_srv_ = nh.advertiseService("inlanecruising_plugin/plan_trajectory", &InLaneCruisingPlugin::plan_trajectory_cb, &worker); //TODO: Update yield client to use the Plugin Manager capabilities query, in case someone else wants to add an alternate yield implementation - ros::ServiceClient yield_client = nh.serviceClient("plugins/YieldPlugin/plan_trajectory"); + ros::ServiceClient yield_client = nh.serviceClient("yield_plugin/plan_trajectory"); worker.set_yield_client(yield_client); ROS_INFO_STREAM("Yield Client Set"); diff --git a/inlanecruising_plugin/src/helper.cpp b/inlanecruising_plugin/src/helper.cpp index 8000797e81..98a0b5108f 100644 --- a/inlanecruising_plugin/src/helper.cpp +++ b/inlanecruising_plugin/src/helper.cpp @@ -34,7 +34,7 @@ int main(int argc, char **argv) ros::init(argc, argv, "helper"); ros::NodeHandle n; - ros::ServiceServer service = n.advertiseService("plugins/YieldPlugin/plan_trajectory", callback); + ros::ServiceServer service = n.advertiseService("yield_plugin/plan_trajectory", callback); ros::spin(); ros::Duration(5).sleep(); diff --git a/inlanecruising_plugin/src/inlanecruising_plugin.cpp b/inlanecruising_plugin/src/inlanecruising_plugin.cpp index 573380e20d..378aeb5902 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin.cpp @@ -42,10 +42,10 @@ InLaneCruisingPlugin::InLaneCruisingPlugin(carma_wm::WorldModelConstPtr wm, InLa PublishPluginDiscoveryCB plugin_discovery_publisher, DebugPublisher debug_publisher) : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher), debug_publisher_(debug_publisher) { - plugin_discovery_msg_.name = "InLaneCruisingPlugin"; + plugin_discovery_msg_.name = "inlanecruising_plugin"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; } diff --git a/inlanecruising_plugin/test/rostest_ilc.cpp b/inlanecruising_plugin/test/rostest_ilc.cpp index 3a5ac9a0ce..6c1da8b1f1 100644 --- a/inlanecruising_plugin/test/rostest_ilc.cpp +++ b/inlanecruising_plugin/test/rostest_ilc.cpp @@ -35,7 +35,7 @@ TEST(InLaneCruisingPluginTest, rostest1) cav_srvs::PlanTrajectory traj_srv; traj_srv.request.initial_trajectory_plan.trajectory_id = "ILCReq"; - ros::ServiceClient plugin1= nh.serviceClient("plugins/InLaneCruisingPlugin/plan_trajectory"); + ros::ServiceClient plugin1= nh.serviceClient("inlanecruising_plugin/plan_trajectory"); ROS_INFO_STREAM("ilc service: " << plugin1.getService()); if(plugin1.waitForExistence(ros::Duration(5.0))) diff --git a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp index 5244ebc0f0..3effec479e 100644 --- a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp @@ -102,7 +102,7 @@ TEST(InLaneCruisingPluginTest, trajectory_from_points_times_orientations) ASSERT_EQ(0, traj_points[2].controller_plugin_name.compare(controller_plugin)); ASSERT_EQ(0, traj_points[3].controller_plugin_name.compare(controller_plugin)); - std::string expected_plugin_name = "InLaneCruisingPlugin"; + std::string expected_plugin_name = "inlanecruising_plugin"; ASSERT_EQ(0, traj_points[0].planner_plugin_name.compare(expected_plugin_name)); ASSERT_EQ(0, traj_points[1].planner_plugin_name.compare(expected_plugin_name)); ASSERT_EQ(0, traj_points[2].planner_plugin_name.compare(expected_plugin_name)); diff --git a/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h b/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h index 7aa5299853..c62cc586e8 100644 --- a/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h +++ b/intersection_transit_maneuvering/include/intersection_transit_maneuvering_node.h @@ -26,7 +26,7 @@ namespace intersection_transit_maneuvering { /** - * \brief ROS node for the InLaneCruisingPlugin + * \brief ROS node for the inlanecruising_plugin */ class IntersectionTransitManeuveringNode { @@ -50,10 +50,10 @@ class IntersectionTransitManeuveringNode plugin_discovery_pub_ = nh_.advertise("plugin_discovery",1); std::shared_ptr srv = std::make_shared(); - ros::ServiceClient trajectory_client = nh_.serviceClient("plugins/InLaneCruisingPlugin/plan_trajectory"); + ros::ServiceClient trajectory_client = nh_.serviceClient("inlanecruising_plugin/plan_trajectory"); srv->set_client(trajectory_client); IntersectionTransitManeuvering worker([&plugin_discovery_pub_](const auto& msg) {plugin_discovery_pub_.publish(msg);}, srv); - trajectory_srv_ = nh_.advertiseService("plugins/IntersectionTransitPlugin/plan_trajectory",&IntersectionTransitManeuvering::plan_trajectory_cb, &worker); + trajectory_srv_ = nh_.advertiseService("intersection_transit_maneuvering/plan_trajectory",&IntersectionTransitManeuvering::plan_trajectory_cb, &worker); if (!trajectory_client.waitForExistence(ros::Duration(20.0))) { throw std::invalid_argument("Required service is not available: " + trajectory_client.getService()); diff --git a/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp b/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp index 9e300847b3..65671275dc 100644 --- a/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp +++ b/intersection_transit_maneuvering/src/intersection_transit_maneuvering.cpp @@ -130,7 +130,7 @@ std::ostream& operator<<(std::ostream& os, cav_msgs::Maneuver m) { IntersectionTransitManeuvering::IntersectionTransitManeuvering(PublishPluginDiscoveryCB plugin_discovery_publisher, std::shared_ptr obj) { - plugin_discovery_msg_.name = "IntersectionTransitPlugin"; + plugin_discovery_msg_.name = "intersection_transit_maneuvering"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; plugin_discovery_msg_.activated = true; diff --git a/lci_strategic_plugin/config/parameters.yaml b/lci_strategic_plugin/config/parameters.yaml index b9640b3e42..7a603c535e 100755 --- a/lci_strategic_plugin/config/parameters.yaml +++ b/lci_strategic_plugin/config/parameters.yaml @@ -50,15 +50,15 @@ mobility_rate : 10.0 # String: The name to use for this plugin during comminications with the arbitrator -strategic_plugin_name : LCIStrategicPlugin +strategic_plugin_name : lci_strategic_plugin # String: The name of the tactical plugin to use for Lane Following trajectory planning # This plugin is used to apply trajectory smoothing algorithm BEFORE entering the intersection if within activation distance -lane_following_plugin_name : LightControlledIntersectionTacticalPlugin +lane_following_plugin_name : light_controlled_intersection_tactical_plugin # String: The name of the plugin to use for stop and wait trajectory planning -stop_and_wait_plugin_name : StopAndWaitPlugin +stop_and_wait_plugin_name : stop_and_wait_plugin # String: The name of the plugin to use for intersection transit trajectory planning # This plugin is used to travel INSIDE the intersection where there is no trajectory smoothing algorithm active -intersection_transit_plugin_name : IntersectionTransitPlugin \ No newline at end of file +intersection_transit_plugin_name : intersection_transit_maneuvering \ No newline at end of file diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h index 64ade7c3f0..130be32287 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin_config.h @@ -84,17 +84,17 @@ struct LCIStrategicPluginConfig std::string vehicle_id = "default_id"; //! The name to use for this plugin during comminications with the arbitrator - std::string strategic_plugin_name = "LCIStrategicPlugin"; + std::string strategic_plugin_name = "lci_strategic_plugin"; //! The name of the tactical plugin to use for Lane Following trajectory planning //! This plugin is used to apply trajectory smoothing algorithm BEFORE entering the intersection if within activation distance - std::string lane_following_plugin_name = "LightControlledIntersectionTacticalPlugin"; + std::string lane_following_plugin_name = "light_controlled_intersection_tactical_plugin"; //! The name of the plugin to use for stop and wait trajectory planning - std::string stop_and_wait_plugin_name = "StopAndWaitPlugin"; + std::string stop_and_wait_plugin_name = "stop_and_wait_plugin"; //! The name of the plugin to use for intersection transit trajectory planning //! This plugin is used to travel INSIDE the intersection where there is no trajectory smoothing algorithm active - std::string intersection_transit_plugin_name = "IntersectionTransitPlugin"; + std::string intersection_transit_plugin_name = "intersection_transit_maneuvering"; }; } // namespace localizer \ No newline at end of file diff --git a/lci_strategic_plugin/src/main.cpp b/lci_strategic_plugin/src/main.cpp index 3f854f1c5c..73f9f3cef2 100755 --- a/lci_strategic_plugin/src/main.cpp +++ b/lci_strategic_plugin/src/main.cpp @@ -83,7 +83,7 @@ int main(int argc, char** argv) // Setup callback connections ros::ServiceServer plan_maneuver_srv = - nh.advertiseService("plugins/" + config.strategic_plugin_name + "/plan_maneuvers", &lci_strategic_plugin::LCIStrategicPlugin::planManeuverCb, &lcip); + nh.advertiseService(config.strategic_plugin_name + "/plan_maneuvers", &lci_strategic_plugin::LCIStrategicPlugin::planManeuverCb, &lcip); lcip.lookupFrontBumperTransform(); diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h index e1a06d5f11..c765cf9133 100755 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h @@ -105,7 +105,7 @@ class LightControlledIntersectionTransitPluginNode LightControlledIntersectionTacticalPlugin worker(wm_, config, [&discovery_pub](auto& msg) { discovery_pub.publish(msg); }); - ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/LightControlledIntersectionTacticalPlugin/plan_trajectory", + ros::ServiceServer trajectory_srv_ = nh.advertiseService("light_controlled_intersection_tactical_plugin/plan_trajectory", &LightControlledIntersectionTacticalPlugin::plan_trajectory_cb, &worker); ros::Timer discovery_pub_timer_ = diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 576f789723..2f002bba9e 100755 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -52,7 +52,7 @@ LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPl const PublishPluginDiscoveryCB& plugin_discovery_publisher) : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher) { - plugin_discovery_msg_.name = "LightControlledIntersectionTacticalPlugin"; + plugin_discovery_msg_.name = "light_controlled_intersection_tactical_plugin"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; plugin_discovery_msg_.activated = true; diff --git a/platooning_control/src/platoon_control.cpp b/platooning_control/src/platoon_control.cpp index ab5f5a9418..bd5e459b72 100644 --- a/platooning_control/src/platoon_control.cpp +++ b/platooning_control/src/platoon_control.cpp @@ -63,7 +63,7 @@ namespace platoon_control config_ = config; // Trajectory Plan Subscriber - trajectory_plan_sub = nh_->subscribe("PlatooningControlPlugin/plan_trajectory", 1, &PlatoonControlPlugin::trajectoryPlan_cb, this); + trajectory_plan_sub = nh_->subscribe("platoon_control/plan_trajectory", 1, &PlatoonControlPlugin::trajectoryPlan_cb, this); // Current Twist Subscriber current_twist_sub_ = nh_->subscribe("current_velocity", 1, &PlatoonControlPlugin::currentTwist_cb, this); @@ -80,7 +80,7 @@ namespace platoon_control pose_sub_ = nh_->subscribe("current_pose", 1, &PlatoonControlPlugin::pose_cb, this); plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); - plugin_discovery_msg_.name = "PlatooningControlPlugin"; + plugin_discovery_msg_.name = "platoon_control"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; plugin_discovery_msg_.activated = true; diff --git a/platooning_control/test/test_mynode.cpp b/platooning_control/test/test_mynode.cpp index e71a13fd49..a84ef4b9c0 100644 --- a/platooning_control/test/test_mynode.cpp +++ b/platooning_control/test/test_mynode.cpp @@ -32,7 +32,7 @@ TEST(TestSuite, testCase1) { ros::NodeHandle nh = ros::NodeHandle(); - ros::Publisher traj_pub_ = nh.advertise("PlatooningControlPlugin/plan_trajectory", 5); + ros::Publisher traj_pub_ = nh.advertise("platoon_control/plan_trajectory", 5); cav_msgs::TrajectoryPlan tp; traj_pub_.publish(tp); std::this_thread::sleep_for(std::chrono::milliseconds(5000)); diff --git a/platooning_control_ihp/src/platoon_control_ihp.cpp b/platooning_control_ihp/src/platoon_control_ihp.cpp index 979facdae7..32b469b0b5 100644 --- a/platooning_control_ihp/src/platoon_control_ihp.cpp +++ b/platooning_control_ihp/src/platoon_control_ihp.cpp @@ -60,7 +60,7 @@ namespace platoon_control_ihp config_ = config; // Trajectory Plan Subscriber - trajectory_plan_sub = nh_->subscribe("PlatoonControlIHPPlugin/plan_trajectory", 1, &PlatoonControlIHPPlugin::trajectoryPlan_cb, this); + trajectory_plan_sub = nh_->subscribe("platoon_control_ihp/plan_trajectory", 1, &PlatoonControlIHPPlugin::trajectoryPlan_cb, this); // Current Twist Subscriber current_twist_sub_ = nh_->subscribe("current_velocity", 1, &PlatoonControlIHPPlugin::currentTwist_cb, this); @@ -78,7 +78,7 @@ namespace platoon_control_ihp pose_sub_ = nh_->subscribe("current_pose", 1, &PlatoonControlIHPPlugin::pose_cb, this); plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); - plugin_discovery_msg_.name = "PlatoonControlIHPPlugin"; + plugin_discovery_msg_.name = "platoon_control_ihp"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; plugin_discovery_msg_.activated = true; diff --git a/platooning_control_ihp/test/test_mynode.cpp b/platooning_control_ihp/test/test_mynode.cpp index 289a48d074..2c8c6b8c04 100644 --- a/platooning_control_ihp/test/test_mynode.cpp +++ b/platooning_control_ihp/test/test_mynode.cpp @@ -32,7 +32,7 @@ TEST(TestSuite, testCase1) { ros::NodeHandle nh = ros::NodeHandle(); - ros::Publisher traj_pub_ = nh.advertise("PlatooningControlPlugin/plan_trajectory", 5); + ros::Publisher traj_pub_ = nh.advertise("platoon_control/plan_trajectory", 5); cav_msgs::TrajectoryPlan tp; traj_pub_.publish(tp); std::this_thread::sleep_for(std::chrono::milliseconds(5000)); diff --git a/platooning_strategic/include/platoon_config.h b/platooning_strategic/include/platoon_config.h index cef8e16e8f..c64366cdc0 100644 --- a/platooning_strategic/include/platoon_config.h +++ b/platooning_strategic/include/platoon_config.h @@ -19,7 +19,7 @@ #include /** - * \brief Stuct containing the algorithm configuration values for the YieldPluginConfig + * \brief Stuct containing the algorithm configuration values for the PlatoonStrategicPlugin */ struct PlatoonPluginConfig { diff --git a/platooning_strategic/include/platoon_strategic_plugin_node.h b/platooning_strategic/include/platoon_strategic_plugin_node.h index 706b2e5394..2fb72a06d9 100644 --- a/platooning_strategic/include/platoon_strategic_plugin_node.h +++ b/platooning_strategic/include/platoon_strategic_plugin_node.h @@ -32,7 +32,7 @@ namespace platoon_strategic { /** - * \brief ROS node for the YieldPlugin + * \brief ROS node for the platoon_strategic plugin */ class PlatoonStrategicPluginNode { @@ -86,7 +86,7 @@ class PlatoonStrategicPluginNode [&mob_request_pub](auto msg) { mob_request_pub.publish(msg); }, [&mob_operation_pub](auto msg) { mob_operation_pub.publish(msg); }, [&platoon_info_pub](auto msg) { platoon_info_pub.publish(msg); } ); - ros::ServiceServer maneuver_srv_ = nh.advertiseService("plugins/PlatooningStrategicPlugin/plan_maneuvers", + ros::ServiceServer maneuver_srv_ = nh.advertiseService("platoon_strategic/plan_maneuvers", &PlatoonStrategicPlugin::plan_maneuver_cb, &worker); ros::Subscriber mob_request_sub = nh.subscribe("incoming_mobility_request", 1, &PlatoonStrategicPlugin::mob_req_cb, &worker); ros::Subscriber mob_response_sub = nh.subscribe("incoming_mobility_response", 1, &PlatoonStrategicPlugin::mob_resp_cb, &worker); diff --git a/platooning_strategic/src/platoon_strategic.cpp b/platooning_strategic/src/platoon_strategic.cpp index ab74558f12..49b61ee736 100644 --- a/platooning_strategic/src/platoon_strategic.cpp +++ b/platooning_strategic/src/platoon_strategic.cpp @@ -36,10 +36,10 @@ namespace platoon_strategic { pm_ = PlatoonManager(); - plugin_discovery_msg_.name = "PlatooningStrategicPlugin"; + plugin_discovery_msg_.name = "platoon_strategic"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::STRATEGIC; plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers"; } @@ -377,8 +377,8 @@ namespace platoon_strategic maneuver_msg.type = cav_msgs::Maneuver::LANE_CHANGE; maneuver_msg.lane_change_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; maneuver_msg.lane_change_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_change_maneuver.parameters.planning_tactical_plugin = "CooperativeLaneChangePlugin"; - maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = "PlatooningStrategicPlugin"; + maneuver_msg.lane_change_maneuver.parameters.planning_tactical_plugin = "cooperative_lanechange"; + maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = "platoon_strategic"; maneuver_msg.lane_change_maneuver.start_dist = start_dist; maneuver_msg.lane_change_maneuver.start_speed = start_speed; maneuver_msg.lane_change_maneuver.end_dist = end_dist; @@ -418,8 +418,8 @@ namespace platoon_strategic maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::PLATOONING; maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "PlatooningTacticalPlugin"; - maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "PlatooningStrategicPlugin"; + maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "platooning_tactical_plugin"; + maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "platoon_strategic"; maneuver_msg.lane_following_maneuver.start_dist = current_dist; maneuver_msg.lane_following_maneuver.start_speed = current_speed; maneuver_msg.lane_following_maneuver.start_time = current_time; diff --git a/platooning_strategic_IHP/include/platoon_config_ihp.h b/platooning_strategic_IHP/include/platoon_config_ihp.h index ce79bbdd16..860f7a043d 100644 --- a/platooning_strategic_IHP/include/platoon_config_ihp.h +++ b/platooning_strategic_IHP/include/platoon_config_ihp.h @@ -19,7 +19,7 @@ #include /** - * \brief Stuct containing the algorithm configuration values for the YieldPluginConfig + * \brief Stuct containing the algorithm configuration values for the yield_pluginConfig */ struct PlatoonPluginConfig { diff --git a/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h index db2be513e2..67c9583e50 100644 --- a/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h +++ b/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h @@ -39,7 +39,7 @@ namespace platoon_strategic_ihp { /** - * \brief ROS node for the YieldPlugin + * \brief ROS node for the yield_plugin */ class PlatoonStrategicIHPPluginNode { @@ -100,7 +100,7 @@ class PlatoonStrategicIHPPluginNode [&mob_request_pub](auto msg) { mob_request_pub.publish(msg); }, [&mob_operation_pub](auto msg) { mob_operation_pub.publish(msg); }, [&platoon_info_pub](auto msg) { platoon_info_pub.publish(msg); } ); - ros::ServiceServer maneuver_srv_ = nh.advertiseService("plugins/PlatooningStrategicIHPPlugin/plan_maneuvers", + ros::ServiceServer maneuver_srv_ = nh.advertiseService("platoon_strategic_ihp/plan_maneuvers", &PlatoonStrategicIHPPlugin::plan_maneuver_cb, &worker); ros::Subscriber mob_request_sub = nh.subscribe("incoming_mobility_request", 1, &PlatoonStrategicIHPPlugin::mob_req_cb, &worker); ros::Subscriber mob_response_sub = nh.subscribe("incoming_mobility_response", 1, &PlatoonStrategicIHPPlugin::mob_resp_cb, &worker); diff --git a/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp index d10e5b5f11..d99e37e109 100644 --- a/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp +++ b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp @@ -54,10 +54,10 @@ namespace platoon_strategic_ihp PlatoonMember hostVehicleMember = PlatoonMember(hostStaticId, 0.0, 0.0, 0.0, 0.0, cur_t); pm_.host_platoon_.push_back(hostVehicleMember); - plugin_discovery_msg_.name = "PlatooningStrategicIHPPlugin"; + plugin_discovery_msg_.name = "platoon_strategic_ihp"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::STRATEGIC; plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers"; ROS_DEBUG_STREAM("ctor complete. hostStaticId = " << hostStaticId); @@ -3142,8 +3142,8 @@ namespace platoon_strategic_ihp maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING; maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::PLATOONING; maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "PlatooningTacticalPlugin"; - maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "PlatooningStrategicIHPPlugin"; + maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "platooning_tactical_plugin"; + maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "platoon_strategic_ihp"; maneuver_msg.lane_following_maneuver.start_dist = current_dist; maneuver_msg.lane_following_maneuver.start_speed = current_speed; maneuver_msg.lane_following_maneuver.start_time = current_time; @@ -3167,8 +3167,8 @@ namespace platoon_strategic_ihp maneuver_msg.type = cav_msgs::Maneuver::LANE_CHANGE; maneuver_msg.lane_change_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::PLATOONING; maneuver_msg.lane_change_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_change_maneuver.parameters.planning_tactical_plugin = "CooperativeLaneChangePlugin"; - maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = "PlatooningStrategicIHPPlugin"; + maneuver_msg.lane_change_maneuver.parameters.planning_tactical_plugin = "cooperative_lanechange"; + maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = "platoon_strategic_ihp"; maneuver_msg.lane_change_maneuver.start_dist = current_dist; maneuver_msg.lane_change_maneuver.start_speed = current_speed; maneuver_msg.lane_change_maneuver.start_time = current_time; diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h index 8785adc9c9..f658c54aff 100644 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h +++ b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h @@ -40,7 +40,7 @@ struct PlatooningTacticalPluginConfig bool enable_object_avoidance = true; // Activate object avoidance logic bool publish_debug = false; // True if debug publishing will be enabled double buffer_ending_downtrack = 20.0; - std::string desired_controller_plugin = "PlatooningControlPlugin"; ////The desired controller plugin for the platooning trajectory + std::string desired_controller_plugin = "platoon_control"; ////The desired controller plugin for the platooning trajectory friend std::ostream& operator<<(std::ostream& output, const PlatooningTacticalPluginConfig& c) { diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h index 5c376e9136..9e7f472e52 100644 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h +++ b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h @@ -73,7 +73,7 @@ class PlatooningTacticalPluginNode PlatooningTacticalPlugin worker(wm_, config, [&discovery_pub](auto msg) { discovery_pub.publish(msg); }); - ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/PlatooningTacticalPlugin/plan_trajectory", + ros::ServiceServer trajectory_srv_ = nh.advertiseService("platooning_tactical_plugin/plan_trajectory", &PlatooningTacticalPlugin::plan_trajectory_cb, &worker); ros::Timer discovery_pub_timer_ = nh.createTimer( diff --git a/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp b/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp index 6def33817b..260cc88e5f 100644 --- a/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp +++ b/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp @@ -40,10 +40,10 @@ PlatooningTacticalPlugin::PlatooningTacticalPlugin(carma_wm::WorldModelConstPtr PublishPluginDiscoveryCB plugin_discovery_publisher) : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher) { - plugin_discovery_msg_.name = "PlatooningTacticalPlugin"; + plugin_discovery_msg_.name = "platooning_tactical_plugin"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; } diff --git a/port_drayage_plugin/src/port_drayage_plugin.cpp b/port_drayage_plugin/src/port_drayage_plugin.cpp index 50546a0f4f..fd26ac908e 100644 --- a/port_drayage_plugin/src/port_drayage_plugin.cpp +++ b/port_drayage_plugin/src/port_drayage_plugin.cpp @@ -259,7 +259,7 @@ namespace port_drayage_plugin maneuver_msg.type = cav_msgs::Maneuver::STOP_AND_WAIT; maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = "StopAndWaitPlugin"; + maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = "stop_and_wait_plugin"; maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "PortDrayageWorkerPlugin"; maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist; maneuver_msg.stop_and_wait_maneuver.start_speed = current_speed; @@ -278,7 +278,7 @@ namespace port_drayage_plugin maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "InlaneCruisingPlugin"; - maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "RouteFollowingPlugin"; + maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "route_following_plugin"; maneuver_msg.lane_following_maneuver.start_dist = current_dist; maneuver_msg.lane_following_maneuver.start_speed = current_speed; maneuver_msg.lane_following_maneuver.start_time = time; diff --git a/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp b/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp index 6566b317ca..f1b2ec11c3 100644 --- a/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp +++ b/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp @@ -25,12 +25,12 @@ namespace pure_pursuit_wrapper PurePursuitWrapper::PurePursuitWrapper(PurePursuitWrapperConfig config, WaypointPub waypoint_pub, PluginDiscoveryPub plugin_discovery_pub) : config_(config), waypoint_pub_(waypoint_pub), plugin_discovery_pub_(plugin_discovery_pub) { - plugin_discovery_msg_.name = "Pure Pursuit"; + plugin_discovery_msg_.name = "pure_pursuit_wrapper_node"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::CONTROL; - plugin_discovery_msg_.capability = "control_pure_pursuit_plan/plan_controls"; + plugin_discovery_msg_.capability = "control/trajectory_control"; } bool PurePursuitWrapper::onSpin() diff --git a/pure_pursuit_wrapper/test/pure_pursuit_wrapper_test.cpp b/pure_pursuit_wrapper/test/pure_pursuit_wrapper_test.cpp index a6e8dfff44..c481ffe22f 100755 --- a/pure_pursuit_wrapper/test/pure_pursuit_wrapper_test.cpp +++ b/pure_pursuit_wrapper/test/pure_pursuit_wrapper_test.cpp @@ -88,12 +88,12 @@ TEST(pure_pursuit_wrapper, onSpin) ASSERT_TRUE(!!plugin_msg); cav_msgs::Plugin msg = plugin_msg.get(); - ASSERT_EQ(0, msg.name.compare("Pure Pursuit")); + ASSERT_EQ(0, msg.name.compare("pure_pursuit_wrapper_node")); ASSERT_EQ(0, msg.version_id.compare("v1.0")); ASSERT_TRUE(msg.available); ASSERT_TRUE(msg.activated); ASSERT_EQ(cav_msgs::Plugin::CONTROL, msg.type); - ASSERT_EQ(0, msg.capability.compare("control_pure_pursuit_plan/plan_controls")); + ASSERT_EQ(0, msg.capability.compare("control/trajectory_control")); } int main(int argc, char** argv) diff --git a/route_following_plugin/config/parameters.yaml b/route_following_plugin/config/parameters.yaml index c075002398..2b2c8ade12 100644 --- a/route_following_plugin/config/parameters.yaml +++ b/route_following_plugin/config/parameters.yaml @@ -10,14 +10,14 @@ stopping_accel_limit_multiplier: 0.5 buffer_lanechange_time : 3.0 # String : The tactical plugin being used for lane change -# For CLC: "CooperativeLaneChangePlugin" for unobstructed : "UnobstructedLaneChangePlugin" -lane_change_plugin : "CooperativeLaneChangePlugin" +# For CLC: "cooperative_lanechange" +lane_change_plugin : "cooperative_lanechange" # String : The tactical plugin being used for stopping at the end of the route -stop_and_wait_plugin : "StopAndWaitPlugin" +stop_and_wait_plugin : "stop_and_wait_plugin" # String : The tactical plugin being used for lane following -lane_following_plugin : "InLaneCruisingPlugin" +lane_following_plugin : "inlanecruising_plugin" # Double : The minimum length a maneuver can be when it is being modified to support stopping behavior. # Units : meters diff --git a/route_following_plugin/include/route_following_plugin.h b/route_following_plugin/include/route_following_plugin.h index 5c04be999d..4d6ab6937f 100644 --- a/route_following_plugin/include/route_following_plugin.h +++ b/route_following_plugin/include/route_following_plugin.h @@ -288,11 +288,11 @@ namespace route_following_plugin std::vector latest_maneuver_plan_; //Tactical plugin being used for planning lane change - std::string lane_change_plugin_ = "CooperativeLaneChangePlugin"; - std::string stop_and_wait_plugin_ = "StopAndWaitPlugin"; + std::string lane_change_plugin_ = "cooperative_lanechange"; + std::string stop_and_wait_plugin_ = "stop_and_wait_plugin"; - std::string planning_strategic_plugin_ = "RouteFollowingPlugin"; - std::string lanefollow_planning_tactical_plugin_ = "InLaneCruisingPlugin"; + std::string planning_strategic_plugin_ = "route_following_plugin"; + std::string lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"; /** * \brief Callback for the front bumper pose transform diff --git a/route_following_plugin/src/route_following_plugin.cpp b/route_following_plugin/src/route_following_plugin.cpp index b81258266e..802667c17f 100644 --- a/route_following_plugin/src/route_following_plugin.cpp +++ b/route_following_plugin/src/route_following_plugin.cpp @@ -88,7 +88,7 @@ void setManeuverLaneletIds(cav_msgs::Maneuver& mvr, lanelet::Id start_id, lanele nh_.reset(new ros::CARMANodeHandle()); pnh_.reset(new ros::CARMANodeHandle("~")); - plan_maneuver_srv_ = nh_->advertiseService("plugins/" + planning_strategic_plugin_ + "/plan_maneuvers", &RouteFollowingPlugin::planManeuverCb, this); + plan_maneuver_srv_ = nh_->advertiseService(planning_strategic_plugin_ + "/plan_maneuvers", &RouteFollowingPlugin::planManeuverCb, this); plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); upcoming_lane_change_status_pub_ = nh_->advertise("upcoming_lane_change_status", 1); diff --git a/route_following_plugin/test/test_route_following_plugin.cpp b/route_following_plugin/test/test_route_following_plugin.cpp index 115e242ec9..b676aa9813 100644 --- a/route_following_plugin/test/test_route_following_plugin.cpp +++ b/route_following_plugin/test/test_route_following_plugin.cpp @@ -44,7 +44,7 @@ namespace route_following_plugin EXPECT_EQ(cav_msgs::Maneuver::LANE_FOLLOWING, msg.type); EXPECT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, msg.lane_following_maneuver.parameters.negotiation_type); EXPECT_EQ(cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN, msg.lane_following_maneuver.parameters.presence_vector); - EXPECT_EQ("InLaneCruisingPlugin", msg.lane_following_maneuver.parameters.planning_tactical_plugin); + EXPECT_EQ("inlanecruising_plugin", msg.lane_following_maneuver.parameters.planning_tactical_plugin); EXPECT_EQ("RouteFollowingPlugin", msg.lane_following_maneuver.parameters.planning_strategic_plugin); EXPECT_NEAR(1.0, msg.lane_following_maneuver.start_dist, 0.01); EXPECT_NEAR(0.9, msg.lane_following_maneuver.start_speed, 0.01); diff --git a/sci_strategic_plugin/config/parameters.yaml b/sci_strategic_plugin/config/parameters.yaml index a9adadc44e..b6176a2e3c 100644 --- a/sci_strategic_plugin/config/parameters.yaml +++ b/sci_strategic_plugin/config/parameters.yaml @@ -20,13 +20,13 @@ reaction_time : 5.0 intersection_exit_zone_length : 6.0 # String: The name to use for this plugin during comminications with the arbitrator -strategic_plugin_name : SCIStrategicPlugin +strategic_plugin_name : sci_strategic_plugin # String: The name of the tactical plugin to use for Lane Following trajectory planning -lane_following_plugin_name : StopControlledIntersectionTacticalPlugin +lane_following_plugin_name : stop_controlled_intersection_tactical_plugin # String: The name of the plugin to use for stop and wait trajectory planning -stop_and_wait_plugin_name : StopAndWaitPlugin +stop_and_wait_plugin_name : stop_and_wait_plugin # String: The name of the plugin to use for intersection transit trajectory planning -intersection_transit_plugin_name : IntersectionTransitPlugin \ No newline at end of file +intersection_transit_plugin_name : intersection_transit_maneuvering \ No newline at end of file diff --git a/sci_strategic_plugin/include/sci_strategic_plugin_config.h b/sci_strategic_plugin/include/sci_strategic_plugin_config.h index 04125ed664..a6ce9ed6c8 100644 --- a/sci_strategic_plugin/include/sci_strategic_plugin_config.h +++ b/sci_strategic_plugin/include/sci_strategic_plugin_config.h @@ -56,16 +56,16 @@ struct SCIStrategicPluginConfig double intersection_exit_zone_length = 15.0; //! The name to use for this plugin during comminications with the arbitrator - std::string strategic_plugin_name = "SCIStrategicPlugin"; + std::string strategic_plugin_name = "sci_strategic_plugin"; //! The name of the tactical plugin to use for Lane Following trajectory planning - std::string lane_following_plugin_name = "StopControlledIntersectionTacticalPlugin"; + std::string lane_following_plugin_name = "stop_controlled_intersection_tactical_plugin"; //! The name of the plugin to use for stop and wait trajectory planning - std::string stop_and_wait_plugin_name = "StopAndWaitPlugin"; + std::string stop_and_wait_plugin_name = "stop_and_wait_plugin"; //! The name of the plugin to use for intersection transit trajectory planning - std::string intersection_transit_plugin_name = "IntersectionTransitPlugin"; + std::string intersection_transit_plugin_name = "intersection_transit_maneuvering"; //! License plate of the vehicle. std::string vehicle_id = "default_id"; diff --git a/sci_strategic_plugin/src/main.cpp b/sci_strategic_plugin/src/main.cpp index ae011909f0..e04ba83cf3 100644 --- a/sci_strategic_plugin/src/main.cpp +++ b/sci_strategic_plugin/src/main.cpp @@ -58,7 +58,7 @@ int main(int argc, char** argv) // Setup callback connections ros::ServiceServer plan_maneuver_srv = - nh.advertiseService("plugins/" + config.strategic_plugin_name + "/plan_maneuvers", &sci_strategic_plugin::SCIStrategicPlugin::planManeuverCb, &plugin); + nh.advertiseService(config.strategic_plugin_name + "/plan_maneuvers", &sci_strategic_plugin::SCIStrategicPlugin::planManeuverCb, &plugin); // Mobility Operation Subscriber ros::Subscriber mob_operation_sub = nh.subscribe("incoming_mobility_operation", 1, &sci_strategic_plugin::SCIStrategicPlugin::mobilityOperationCb, &plugin); diff --git a/stop_and_wait_plugin/include/stop_and_wait_config.h b/stop_and_wait_plugin/include/stop_and_wait_config.h index c26740c69d..b70b1665a0 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_config.h +++ b/stop_and_wait_plugin/include/stop_and_wait_config.h @@ -19,7 +19,7 @@ #include /** - * \brief Stuct containing the algorithm configuration values for the InLaneCruisingPlugin + * \brief Stuct containing the algorithm configuration values for the stop_and_wait_plugin */ struct StopandWaitConfig { diff --git a/stop_and_wait_plugin/launch/stop_and_wait_plugin.launch b/stop_and_wait_plugin/launch/stop_and_wait_plugin.launch index 8f719624e9..f9538ca300 100644 --- a/stop_and_wait_plugin/launch/stop_and_wait_plugin.launch +++ b/stop_and_wait_plugin/launch/stop_and_wait_plugin.launch @@ -15,7 +15,7 @@ --> - + \ No newline at end of file diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp index 6d22822a46..ca5786339b 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -53,10 +53,10 @@ StopandWait::StopandWait(carma_wm::WorldModelConstPtr wm, StopandWaitConfig conf PublishPluginDiscoveryCB plugin_discovery_publisher) : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher) { - plugin_discovery_msg_.name = "StopAndWaitPlugin"; + plugin_discovery_msg_.name = "stop_and_wait_plugin"; plugin_discovery_msg_.version_id = "v1.1"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; }; diff --git a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h index 73f79e6bef..6764f8b8ee 100644 --- a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h +++ b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h @@ -85,7 +85,7 @@ class StopControlledIntersectionTransitPluginNode StopControlledIntersectionTacticalPlugin worker(wm_, config, [&discovery_pub](auto& msg) { discovery_pub.publish(msg); }); // [trajectory_debug_pub](const auto& msg) { trajectory_debug_pub.publish(msg); }); - ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/StopControlledIntersectionTacticalPlugin/plan_trajectory", + ros::ServiceServer trajectory_srv_ = nh.advertiseService("stop_controlled_intersection_tactical_plugin/plan_trajectory", &StopControlledIntersectionTacticalPlugin::plan_trajectory_cb, &worker); ros::Timer discovery_pub_timer_ = diff --git a/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp b/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp index 3da7c039b2..3e5ada43fc 100644 --- a/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp +++ b/stop_controlled_intersection_tactical_plugin/src/stop_controlled_intersection_tactical_plugin.cpp @@ -53,7 +53,7 @@ StopControlledIntersectionTacticalPlugin::StopControlledIntersectionTacticalPlug const PublishPluginDiscoveryCB& plugin_discovery_publisher) : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher) { - plugin_discovery_msg_.name = "StopControlledIntersectionTacticalPlugin"; + plugin_discovery_msg_.name = "stop_controlled_intersection_tactical_plugin"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; plugin_discovery_msg_.activated = true; diff --git a/subsystem_controllers/CMakeLists.txt b/subsystem_controllers/CMakeLists.txt index b8aee952b4..74bb2ed31e 100644 --- a/subsystem_controllers/CMakeLists.txt +++ b/subsystem_controllers/CMakeLists.txt @@ -35,10 +35,16 @@ ament_auto_add_executable(localization_controller src/localization_controller/ma target_link_libraries(localization_controller localization_controller_core ${base_lib}) # Guidance Subsystem -ament_auto_add_library(guidance_controller_core SHARED src/guidance_controller/guidance_controller.cpp) +ament_auto_add_library(guidance_controller_core SHARED + src/guidance_controller/guidance_controller.cpp + src/guidance_controller/entry_manager.cpp + src/guidance_controller/plugin_manager.cpp +) rclcpp_components_register_nodes(guidance_controller_core "subsystem_controllers::GuidanceControllerNode") target_link_libraries(guidance_controller_core ${base_lib}) -ament_auto_add_executable(guidance_controller src/guidance_controller/main.cpp) +ament_auto_add_executable(guidance_controller + src/guidance_controller/main.cpp +) target_link_libraries(guidance_controller guidance_controller_core ${base_lib}) # Environment Subsystem diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 224906401f..5a716f77c2 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -30,4 +30,28 @@ unmanaged_required_nodes: [''] # TODO add the controller driver once it is integrated with ROS2 # 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: true \ No newline at end of file + full_subsystem_required: false + + # List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. + # Required plugins will be automatically activated at startup + # Required plugins cannot be deactivated by the user + required_plugins: + - /guidance/plugins/route_following_plugin + - /guidance/plugins/pure_pursuit_wrapper + - /guidance/plugins/inlane_cruising_plugin + - /guidance/plugins/cooperative_lanechange + + # List of guidance plugins which are not required but the user wishes to have automatically activated + # so that the user doesn't need to manually activate them via the UI on each launch (though they still can) + # this list should have zero intersection with the required_plugins + auto_activated_plugins: + - /guidance/plugins/lic_strategic_plugin + - /guidance/plugins/intersection_transit_maneuvering + - /guidance/plugins/light_controlled_intersection_tactical_plugin + - /guidance/plugins/stop_and_wait_plugin + - /guidance/plugins/sci_strategic_plugin + - /guidance/plugins/stop_controlled_intersection_tactical_plugin + - /guidance/plugins/platoon_control_ihp + - /guidance/plugins/platoon_strategic_ihp + - /guidance/plugins/platooning_tactical_plugin + - /guidance/plugins/yield_plugin \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp b/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp index 48272a39fe..00c1e42911 100644 --- a/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp @@ -94,6 +94,17 @@ namespace subsystem_controllers //! The configuration struct BaseSubSystemControllerConfig base_config_; + + //! Collection of flags which, if true, will cause the base class to make lifecycle service calls to managed nodes + // when ever the respective handle_on_ methods (ie. handle_on_configure) are called. + // by setting these flags to false an extending class chooses to implement that call itself. + // flags for on_shutdown and on_error are explicitly not provided since either should always result in subsystem shutdown. + // Overriding the respective methods without calling the base version will achieve the same external behavior but will also result in some internal variables not being populated. + // For correct behavior these flags should be set in the constructor + bool trigger_managed_nodes_configure_from_base_class_ = true; + bool trigger_managed_nodes_activate_from_base_class_ = true; + bool trigger_managed_nodes_deactivate_from_base_class_ = true; + bool trigger_managed_nodes_cleanup_from_base_class_ = true; }; } // namespace subsystem_controllers diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h new file mode 100644 index 0000000000..c39bb7d84f --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h @@ -0,0 +1,52 @@ +#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 + +namespace subsystem_controllers +{ + /** + * \brief An entry represents a plugins details for the purposes of tracking + */ + struct Entry + { + //! Availability flag of a plugin + bool available_ = false; + //! Activation flag of a plugin + bool active_ = false; + //! Fully specified node name of a plugin + std::string name_; + //! Type of the plugin from the message enum in carma_planning_msgs::Plugin + uint8_t type_ = 0; + //! The capability string of the plugin + std::string capability_; + //! Flag indicating if the user requested this plugin be activated + bool user_requested_activation_ = false; + //! Flag indicating if this is a ros1 node + bool is_ros1_ = false; + + /** + * \brief All fields constructor + */ + Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability, bool user_requested_activation) + : available_(available), active_(active), name_(name), type_(type), capability_(capability), user_requested_activation_(user_requested_activation) {} + + + Entry() = default; + }; +} \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h new file mode 100644 index 0000000000..4dd5d870ca --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry_manager.h @@ -0,0 +1,72 @@ +#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 "entry.h" + +namespace subsystem_controllers +{ + /** + * \brief An entry manager keeps track of the set of entries and makes it easy to add or remove entries + */ + class EntryManager + { + public: + + /*! + * \brief Default constructor for EntryManager. + */ + EntryManager() = default; + + /*! + * \brief Add a new entry if the given name does not exist. + * Update an existing entry if the given name exists. + * + * \param entry The entry to update or add + */ + void update_entry(const Entry& entry); + + /*! + * \brief Get all entries as a list. + */ + std::vector get_entries() const; + + /*! + * \brief Get all entry names as a list + */ + std::vector get_entry_names() 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); + + private: + + //! private map by entry name to keep track of all entries + std::unordered_map entry_map_; + + }; +} \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp index 7eb9934f5b..227154e4c7 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller.hpp @@ -20,13 +20,20 @@ #include -#include "carma_msgs/msg/system_alert.hpp" -#include "ros2_lifecycle_manager/ros2_lifecycle_manager.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +#include #include "subsystem_controllers/base_subsystem_controller/base_subsystem_controller.hpp" +#include "plugin_manager.h" +#include "guidance_controller_config.hpp" namespace subsystem_controllers { + namespace cr2 = carma_ros2_utils; class GuidanceControllerNode : public BaseSubsystemController { @@ -43,8 +50,38 @@ namespace subsystem_controllers */ explicit GuidanceControllerNode(const rclcpp::NodeOptions &options); - // TODO The ROS1 plugin manager functionality should be updated to properly interact with or exist in this node or a guidance_plugin_controller node - // https://github.com/usdot-fhwa-stol/carma-platform/issues/1499 + cr2::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + + cr2::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &); + + cr2::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &); + + cr2::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &); + + cr2::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &); + + private: + //! Plugin manager to handle all the plugin specific discovery and reporting + std::shared_ptr plugin_manager_; + + //! Config for user provided parameters + GuidanceControllerConfig config_; + + //! ROS handles + + cr2::SubPtr plugin_discovery_sub_; + + cr2::ServicePtr get_registered_plugins_server_; + + cr2::ServicePtr get_active_plugins_server_; + + cr2::ServicePtr activate_plugin_server_; + + cr2::ServicePtr get_strategic_plugins_by_capability_server_; + + cr2::ServicePtr get_tactical_plugins_by_capability_server_; + + cr2::ServicePtr get_control_plugins_by_capability_server_; }; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp new file mode 100644 index 0000000000..324c2ee32d --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp @@ -0,0 +1,61 @@ +#pragma once + +/* + * Copyright (C) 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 + +namespace subsystem_controllers +{ + /** + * \brief Stuct containing the algorithm configuration values for the GuidanceController + */ + struct GuidanceControllerConfig + + { + //! List of guidance plugins (node name) to consider required and who's failure shall result in automation abort. + // Required plugins will be automatically activated at startup + // Required plugins cannot be deactivated by the user + std::vector required_plugins; + + //! List of guidance plugins which are not required but the user wishes to have automatically activated + // so that the user doesn't need to manually activate them via the UI on each launch (though they still can) + // this list should have zero intersection with the required_plugins + std::vector auto_activated_plugins; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const GuidanceControllerConfig &c) + { + + output << "GuidanceControllerConfig { " << std::endl + << "required_plugins: [ " << std::endl; + + for (auto node : c.required_plugins) + output << node << " "; + + output << "] " << std::endl << "auto_activated_plugins: [ "; + + for (auto node : c.auto_activated_plugins) + output << node << " "; + + output << "] " << std::endl + << "}" << std::endl; + return output; + } + }; + +} // namespace subsystem_controllers diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h new file mode 100644 index 0000000000..3721c45448 --- /dev/null +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -0,0 +1,224 @@ +#pragma once + +/* + * Copyright (C) 2019-2022LEIDOS. + * + * 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.h" +#include "entry.h" + + +namespace subsystem_controllers +{ + + using GetParentNodeStateFunc = std::function; + using SrvHeader = const std::shared_ptr; + /** + * \brief Function which will return a map of service names and their message types based on the provided base node name and namespace + */ + using ServiceNamesAndTypesFunc = std::function>>(const std::string &,const std::string &)>; + + /** + * \brief The PluginManager serves as a component to manage CARMA Guidance Plugins via their ros2 lifecycle interfaces + */ + class PluginManager + { + public: + + // TODO do we need a system alert callback in this manager as well + + /** + * \brief Constructor for PluginManager + * + * \param required_plugins The set of plugins which will be treated as required. A failure in these plugins will result in an exception + * \param auto_activated_plugins The set of plugins which will be automatically activated at first system activation but not treated specially after that. + * \param plugin_lifecycle_mgr A fully initialized lifecycle manager which will be used trigger plugin transitions + * \param get_parent_state_func A callback which will allow this object to access the parent process lifecycle state + * \param get_service_names_and_types_func A callback which returns a map of service names to service types based on the provided base node name and namespace + * \param service_timeout The timeout for plugin services to be available in nanoseconds + * \param call_timeout The timeout for calls to plugin services to fail in nanoseconds + */ + PluginManager(const std::vector& required_plugins, + const std::vector& auto_activated_plugins, + std::shared_ptr plugin_lifecycle_mgr, + GetParentNodeStateFunc get_parent_state_func, + ServiceNamesAndTypesFunc get_service_names_and_types_func, + std::chrono::nanoseconds service_timeout, std::chrono::nanoseconds call_timeout); + + /** + * Below are the state transition methods which will cause this manager to trigger the corresponding + * state transitions in the managed plugins. + * + * \throw std::runtime_error If a required node could not transition successfully + * \return True if all components transitioned successfully + */ + bool configure(); + bool activate(); + bool deactivate(); + bool cleanup(); + bool shutdown(); + + /** + * \brief Update the status of a certain plugin + * + * \param msg A plugin status message + */ + void update_plugin_status(carma_planning_msgs::msg::Plugin::UniquePtr msg); + + /** + * \brief Returns the list of known plugins + * + * \param req The req details + * \param[out] res The response containing the list of known plugins + */ + void get_registered_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); + + /** + * \brief Get the list of currently active plugins + * + * \param header Middle ware header + * \param req The req details + * \param[out] res The response containing the list of active plugins + */ + void get_active_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr req, carma_planning_msgs::srv::PluginList::Response::SharedPtr res); + + /** + * \brief Activate the specified plugin + * + * \param header Middle ware header + * \param req The req details containing the plugin to activate + * \param[out] res The response containing the success flag + */ + void activate_plugin(SrvHeader, carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res); + + /** + * \brief Get strategic plugins by capability + * + * \param header Middle ware header + * \param req The req which identifies which capability is required + * \param res The res which identifies the strategic plugins with the requested capability + */ + void get_strategic_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); + + /** + * \brief Get tactical plugins by capability + * + * \param header Middle ware header + * \param req The req which identifies which capability is required + * \param res The res which identifies the tactical plugins with the requested capability + */ + void get_tactical_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); + + /** + * \brief Get control plugins by capability + * + * \param header Middle ware header + * \param req The req which identifies which capability is required + * \param res The res which identifies the control plugins with the requested capability + */ + void get_control_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res); + + protected: + + /** + * \brief Add the specified entry to our plugin management + * This function will attempt to move the newly detected plugin to the required state + * based on this nodes own state + * + * \param plugin The entry representing the plugin to add + * + * \throws std::runtime_error if this was a required plugin and it could not be transitioned as needed + */ + void add_plugin(const Entry& plugin); + + /** + * \brief Returns true if the provided base capability hierarchy can achieve the requested capability hierarchy + * A capability hierarchy is described as a list of strings where the first string is the most generic description + * of the capability while the last string the is the most detailed description. + * + * For example the capability "tactical_plan/plan_trajectory" would be described as ["tactical_plan", "plan_trajectory"] + * if the user wanted to match this to "tactical_plan/plan_trajectory/platooning_trajectory" then the compared_capability_levels + * would be ["tactical_plan", "plan_trajectory", "compared_capability_levels"] + * Matching those two inputs as ( ["tactical_plan", "plan_trajectory"], ["tactical_plan", "plan_trajectory", "compared_capability_levels"]) + * would be false because the compared_capability_levels is more detailed than the base. + * By contrast, matching ( ["tactical_plan", "plan_trajectory", "compared_capability_levels"], ["tactical_plan", "plan_trajectory"] ) + * would be true since the base is more generic then the request + * + * // TODO check with Kyle on this because it maybe should be the reverse since a more specific capability may require specific meta data. + * + * \param base_capability_levels The base hierarchy to check for compatability + * \param compared_capability_levels The compared_capability_levels which are being checked against the base + * + * \return True if base_capability_levels supports compared_capability_levels + */ + bool matching_capability(const std::vector& base_capability_levels, const std::vector& compared_capability_levels); + + /** + * \brief Returns true if the specified fully qualified node name is a ROS2 lifecycle node + * + * \param node The fully specified name of the node to evaluate + * + * \return True if ros2 lifecycle node. False otherwise + */ + bool is_ros2_lifecycle_node(const std::string& node); + + //! Set of required plugins a failure of which necessitates system shutdown + std::unordered_set required_plugins_; + + //! Set of use specified auto activated plugins which will automatically started without need for user input + // These will only be activated once, if the user later deactivates them then that behavior will be preserved + std::unordered_set auto_activated_plugins_; + + //! Lifecycle Manager which will track the plugin nodes and call their lifecycle services on request + std::shared_ptr plugin_lifecycle_mgr_; + + //! Callback to retrieve the lifecycle state of the parent process + GetParentNodeStateFunc get_parent_state_func_; + + //! Callback to get service names and types for the given node + ServiceNamesAndTypesFunc get_service_names_and_types_func_; + + //! Entry manager to keep track of detected plugins + EntryManager em_; + + //! The timeout for services to be available + std::chrono::nanoseconds service_timeout_; + + //! The timeout for service calls to return + std::chrono::nanoseconds call_timeout_; + + //! Base service name of plan_trajectory service + const std::string plan_maneuvers_suffix_ = "/plan_maneuvers"; + + //! Base service name of plan_trajectory service + const std::string plan_trajectory_suffix_ = "/plan_trajectory"; + + //! Base topic name of control plugin trajectory input topic + const std::string control_trajectory_suffix_ = "/plan_trajectory"; + + }; +} \ No newline at end of file diff --git a/subsystem_controllers/package.xml b/subsystem_controllers/package.xml index bdbb0e8490..d234e2f38a 100644 --- a/subsystem_controllers/package.xml +++ b/subsystem_controllers/package.xml @@ -29,6 +29,7 @@ carma_cmake_common carma_ros2_utils carma_msgs + carma_planning_msgs lifecycle_msgs rclcpp ros2_lifecycle_manager diff --git a/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp b/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp index 5a223c2829..3ed1682fff 100644 --- a/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp +++ b/subsystem_controllers/src/base_subsystem_controller/base_subsystem_controller.cpp @@ -143,6 +143,10 @@ namespace subsystem_controllers } + if (!trigger_managed_nodes_configure_from_base_class_) { + return CallbackReturn::SUCCESS; + } + // With all of our managed nodes now being tracked we can execute their configure operations bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); @@ -164,6 +168,10 @@ namespace subsystem_controllers { RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to activate"); + if (!trigger_managed_nodes_activate_from_base_class_) { + return CallbackReturn::SUCCESS; + } + bool success = lifecycle_mgr_.activate(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); if (success) @@ -184,6 +192,10 @@ namespace subsystem_controllers { RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to deactivate"); + if (!trigger_managed_nodes_deactivate_from_base_class_) { + return CallbackReturn::SUCCESS; + } + bool success = lifecycle_mgr_.deactivate(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); if (success) @@ -204,6 +216,10 @@ namespace subsystem_controllers { RCLCPP_INFO_STREAM(get_logger(), "Subsystem trying to cleanup"); + if (!trigger_managed_nodes_cleanup_from_base_class_) { + return CallbackReturn::SUCCESS; + } + bool success = lifecycle_mgr_.cleanup(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); if (success) diff --git a/subsystem_controllers/src/guidance_controller/entry_manager.cpp b/subsystem_controllers/src/guidance_controller/entry_manager.cpp new file mode 100644 index 0000000000..b1e396d66e --- /dev/null +++ b/subsystem_controllers/src/guidance_controller/entry_manager.cpp @@ -0,0 +1,67 @@ +/* + * 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 "subsystem_controllers/guidance_controller/entry_manager.h" + +namespace subsystem_controllers +{ + + void EntryManager::update_entry(const Entry& entry) + { + entry_map_[entry.name_] = entry; + } + + + std::vector EntryManager::get_entries() const + { + // returns the copy of the original data + std::vector entries; + entries.reserve(entry_map_.size()); + + for (const auto& e : entry_map_) + entries.push_back(e.second); + + return entries; + } + + std::vector EntryManager::get_entry_names() const + { + std::vector names; + names.reserve(entry_map_.size()); + + for (const auto& e : entry_map_) + names.push_back(e.second.name_); + + return names; + } + + void EntryManager::delete_entry(const std::string& name) + { + if (entry_map_.find(name) != entry_map_.end()) + entry_map_.erase(name); + } + + boost::optional EntryManager::get_entry_by_name(const std::string& name) const + { + if (entry_map_.find(name) != entry_map_.end()) + return entry_map_.at(name); + + + // use boost::optional because requested entry might not exist + return boost::none; + } + +} diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 980173701e..4f95f1578d 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -15,15 +15,239 @@ * the License. */ +#include #include "subsystem_controllers/guidance_controller/guidance_controller.hpp" +using std_msec = std::chrono::milliseconds; + namespace subsystem_controllers { GuidanceControllerNode::GuidanceControllerNode(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; } + cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { + auto base_return = BaseSubsystemController::handle_on_configure(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not configure"); + return base_return; + } + + config_ = GuidanceControllerConfig(); + + auto base_managed_nodes = lifecycle_mgr_.get_managed_nodes(); + + std::string plugin_namespace = base_config_.subsystem_namespace + "/plugins/"; + + + std::vector guidance_plugin_nodes; + // Extract the nodes under the plugin namespaces (ie. /guidance/plugins/) + std::copy_if(base_managed_nodes.begin(), base_managed_nodes.end(), + std::back_inserter(guidance_plugin_nodes), + [plugin_namespace](const std::string& s) { return s.rfind(plugin_namespace, 0) == 0; }); + + std::vector non_plugin_guidance_nodes = get_non_intersecting_set(base_managed_nodes, guidance_plugin_nodes); + + lifecycle_mgr_.set_managed_nodes(non_plugin_guidance_nodes); + + // Load required plugins and default enabled plugins + get_parameter>("required_plugins", config_.required_plugins); + get_parameter>("auto_activated_plugins", config_.auto_activated_plugins); + + RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_); + + // The core need is that plugins need to be managed separately from guidance nodes + auto plugin_lifecycle_manager = std::make_shared( + get_node_base_interface(), get_node_graph_interface(), get_node_logging_interface(), get_node_services_interface()); + + plugin_manager_ = std::make_shared( + config_.required_plugins, + config_.auto_activated_plugins, + plugin_lifecycle_manager, + [this](){ return get_current_state().id(); }, + [this](auto node, auto ns){ return get_service_names_and_types_by_node(node, ns); }, + std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms) + ); + + plugin_discovery_sub_ = create_subscription( + "plugin_discovery", 50, + std::bind(&PluginManager::update_plugin_status, plugin_manager_, std::placeholders::_1)); + + get_registered_plugins_server_ = create_service( + "plugins/get_registered_plugins", + std::bind(&PluginManager::get_registered_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_active_plugins_server_ = create_service( + "plugins/get_active_plugins", + std::bind(&PluginManager::get_active_plugins, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + activate_plugin_server_ = create_service( + "plugins/activate_plugin", // TODO check topic name + std::bind(&PluginManager::activate_plugin, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_strategic_plugins_by_capability_server_ = create_service( + "plugins/get_strategic_plugins_by_capability", + std::bind(&PluginManager::get_strategic_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_tactical_plugins_by_capability_server_ = create_service( + "plugins/get_tactical_plugins_by_capability", + std::bind(&PluginManager::get_tactical_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + get_control_plugins_by_capability_server_ = create_service( + "plugins/get_control_plugins_by_capability", + std::bind(&PluginManager::get_control_plugins_by_capability, plugin_manager_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + + + + // With all of our non-plugin managed nodes now being tracked we can execute their configure operations + bool success = lifecycle_mgr_.configure(std_msec(base_config_.service_timeout_ms), std_msec(base_config_.call_timeout_ms)).empty(); + + // Configure our plugins + try { + plugin_manager_->configure(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + + + + 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; + } + + } + + cr2::CallbackReturn GuidanceControllerNode::handle_on_activate(const rclcpp_lifecycle::State &prev_state) + { + auto base_return = BaseSubsystemController::handle_on_activate(prev_state); // This will activate all nodes in the namespace TODO what about the plugins? + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not activate"); + return base_return; + } + + bool success = true; + try { + plugin_manager_->activate(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to activate"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to activate"); + return CallbackReturn::FAILURE; + } + + } + + cr2::CallbackReturn GuidanceControllerNode::handle_on_deactivate(const rclcpp_lifecycle::State &prev_state) + { + auto base_return = BaseSubsystemController::handle_on_deactivate(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not deactivate"); + return base_return; + } + + bool success = true; + try { + plugin_manager_->deactivate(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to deactivate"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to deactivate"); + return CallbackReturn::FAILURE; + } + } + + cr2::CallbackReturn GuidanceControllerNode::handle_on_cleanup(const rclcpp_lifecycle::State &prev_state) + { + auto base_return = BaseSubsystemController::handle_on_cleanup(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not cleanup"); + return base_return; + } + + bool success = true; + try { + plugin_manager_->cleanup(); // Only checking required nodes. Other node failure tracked by activity status + } catch(const std::runtime_error& e) { + success = false; + } + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to cleanup"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to cleanup"); + return CallbackReturn::FAILURE; + } + } + + cr2::CallbackReturn GuidanceControllerNode::handle_on_shutdown(const rclcpp_lifecycle::State &prev_state) + { + auto base_return = BaseSubsystemController::handle_on_shutdown(prev_state); + + if (base_return != cr2::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_logger(), "Guidance Controller could not shutdown"); + return base_return; + } + + bool success = plugin_manager_->shutdown(); + + if (success) + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem able to shutdown cleanly"); + return CallbackReturn::SUCCESS; + } + else + { + + RCLCPP_INFO_STREAM(get_logger(), "Subsystem unable to shutdown cleanly"); + return CallbackReturn::FAILURE; + } + } + + } // namespace subsystem_controllers #include "rclcpp_components/register_node_macro.hpp" diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp new file mode 100644 index 0000000000..3dd8fa0da3 --- /dev/null +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -0,0 +1,532 @@ +/* + * 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 "subsystem_controllers/guidance_controller/plugin_manager.h" + +using std_msec = std::chrono::milliseconds; + +namespace subsystem_controllers +{ + + PluginManager::PluginManager(const std::vector& required_plugins, + const std::vector& auto_activated_plugins, + std::shared_ptr plugin_lifecycle_mgr, + GetParentNodeStateFunc get_parent_state_func, + ServiceNamesAndTypesFunc get_service_names_and_types_func, + std::chrono::nanoseconds service_timeout, std::chrono::nanoseconds call_timeout) + : required_plugins_(required_plugins.begin(), required_plugins.end()), + auto_activated_plugins_(auto_activated_plugins.begin(), auto_activated_plugins.end()), + plugin_lifecycle_mgr_(plugin_lifecycle_mgr), get_parent_state_func_(get_parent_state_func), + get_service_names_and_types_func_(get_service_names_and_types_func), + service_timeout_(service_timeout), call_timeout_(call_timeout) + { + if (!plugin_lifecycle_mgr) + throw std::invalid_argument("Input plugin_lifecycle_mgr to PluginManager constructor cannot be null"); + + // For all required and auto activated plugins add unknown entries but with + // user_requested_activation set to true. + // This will be used later to determine how to transition the plugin specified by that entry + for (const auto& p : required_plugins_) { + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + em_.update_entry(e); + plugin_lifecycle_mgr_->add_managed_node(p); + } + + for (const auto& p : auto_activated_plugins_) { + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + em_.update_entry(e); + plugin_lifecycle_mgr_->add_managed_node(p); + } + + } + + bool PluginManager::is_ros2_lifecycle_node(const std::string& node) + { + // Determine if this plugin is a ROS1 or ROS2 plugin + std::vector name_parts; + boost::split(name_parts, node, boost::is_any_of("/")); + + if (name_parts.empty()) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("subsystem_controllers"), "Invalid name for plugin: " << node << " Plugin may not function in system."); + return false; + } + + std::string base_name = name_parts.back(); + name_parts.pop_back(); + std::string namespace_joined = boost::algorithm::join(name_parts, "/"); + + std::map> services_and_types; + try { + services_and_types = get_service_names_and_types_func_(base_name, namespace_joined); + } + catch (const std::runtime_error& e) { + return false; // Seems this method can throw an exception if not a ros2 node + } + + + + // Next we check if both services are available with the correct type + // Short variable names used here to make conditional more readable + const std::string cs_srv = node + "/change_state"; + const std::string gs_srv = node + "/get_state"; + + if (services_and_types.find(cs_srv) != services_and_types.end() + && services_and_types.find(gs_srv) != services_and_types.end() + && std::find(services_and_types.at(cs_srv).begin(), services_and_types.at(cs_srv).end(), "lifecycle_msgs/srv/ChangeState") != services_and_types.at(cs_srv).end() + && std::find(services_and_types.at(gs_srv).begin(), services_and_types.at(gs_srv).end(), "lifecycle_msgs/srv/GetState") != services_and_types.at(gs_srv).end()) + { + + return true; + + } + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("subsystem_controllers"), "Detected non-ros2 lifecycle plugin " << node); + return false; + } + + void PluginManager::add_plugin(const Entry& plugin) + { + + // If this is a ros1 node we will still track it but we will not attempt to manage its state machine + if (!is_ros2_lifecycle_node(plugin.name_)) { + + Entry ros1_plugin = plugin; + + ros1_plugin.is_ros1_ = true; + + em_.update_entry(ros1_plugin); + + return; + } + + plugin_lifecycle_mgr_->add_managed_node(plugin.name_); + + em_.update_entry(plugin); + + Entry deactivated_entry = plugin; + + auto parent_node_state = get_parent_state_func_(); + + // If this node is not in the active or inactive states then move the plugin to unconfigured + if (parent_node_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE + && parent_node_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + + deactivated_entry.active_ = false; + + // Move plugin to match this nodes state + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(parent_node_state, plugin.name_, service_timeout_, call_timeout_); + + if(result_state != parent_node_state) + { + + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Newly discovered required plugin " + plugin.name_ + " could not be brought to controller node state."); + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to transition newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); + + deactivated_entry.available_ = false; + + } + + em_.update_entry(deactivated_entry); + return; + } + + + // If the node is active or inactive then, + // when adding a plugin, it should be brought to the inactive state + // We do not need transition it beyond inactive in this function as that will be managed by the plugin activation process via UI or parameters + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, service_timeout_, call_timeout_); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Newly discovered required plugin " + plugin.name_ + " could not be brought to inactive state."); + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); + + deactivated_entry.available_ = false; + } + + deactivated_entry.active_ = false; + em_.update_entry(deactivated_entry); + + } + + bool PluginManager::configure() + { + bool full_success = true; + // Bring all known plugins to the inactive state + for (auto plugin : em_.get_entries()) + { + if (plugin.is_ros1_) // We do not manage lifecycle of ros1 nodes + continue; + + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, service_timeout_, call_timeout_); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be configured."); + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to configure newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); + + Entry deactivated_entry = plugin; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; + deactivated_entry.user_requested_activation_ = false; + em_.update_entry(deactivated_entry); + + full_success = false; + } + + } + + return full_success; + + } + + bool PluginManager::activate() + { + bool full_success = true; + // Bring all required or auto activated plugins to the active state + for (auto plugin : em_.get_entries()) + { + if (plugin.is_ros1_) // We do not manage lifecycle of ros1 nodes + continue; + + // If this is not a plugin slated for activation then continue and leave up to user to activate manually later + if (!plugin.user_requested_activation_) + continue; + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, plugin.name_, service_timeout_, call_timeout_); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be activated."); + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to activate newly discovered non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); + + Entry deactivated_entry = plugin; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; + deactivated_entry.user_requested_activation_ = false; + em_.update_entry(deactivated_entry); + + full_success = false; + } + + // If this was an auto activated plugin and not required then we only activate is once + if (auto_activated_plugins_.find(plugin.name_) != auto_activated_plugins_.end()) + { + plugin.user_requested_activation_ = false; + } + + plugin.active_ = true; // Mark plugin as active + + em_.update_entry(plugin); + + } + + return full_success; + } + + bool PluginManager::deactivate() + { + bool full_success = true; + // Bring all required or auto activated plugins to the active state + for (auto plugin : em_.get_entries()) + { + + if (plugin.is_ros1_) // We do not manage lifecycle of ros1 nodes + continue; + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, plugin.name_, service_timeout_, call_timeout_); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be deactivated."); + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to deactivate non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); + + Entry deactivated_entry = plugin; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; + deactivated_entry.user_requested_activation_ = false; + em_.update_entry(deactivated_entry); + + full_success = false; + } + + } + + return full_success; + } + + bool PluginManager::cleanup() + { + bool full_success = true; + // Bring all required or auto activated plugins to the unconfigured state + for (auto plugin : em_.get_entries()) + { + + if (plugin.is_ros1_) // We do not manage lifecycle of ros1 nodes + continue; + + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, plugin.name_, service_timeout_, call_timeout_); + + if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + // If this plugin was required then trigger exception + if (required_plugins_.find(plugin.name_) != required_plugins_.end()) + { + throw std::runtime_error("Required plugin " + plugin.name_ + " could not be cleaned up."); + } + + // If this plugin was not required log an error and mark it is unavailable and deactivated + RCLCPP_ERROR_STREAM(rclcpp::get_logger("subsystem_controllers"), "Failed to cleanup non-required plugin: " + << plugin.name_ << " Marking as deactivated and unavailable!"); + + Entry deactivated_entry = plugin; + deactivated_entry.active_ = false; + deactivated_entry.available_ = false; + deactivated_entry.user_requested_activation_ = false; + em_.update_entry(deactivated_entry); + + full_success = false; + } + + } + + return full_success; + } + + bool PluginManager::shutdown() + { + std::vector all_names = em_.get_entry_names(); + std::vector ros2_names; + ros2_names.reserve(all_names.size()); + + std::copy_if(all_names.begin(), all_names.end(), + std::back_inserter(ros2_names), + [this](const std::string& n) { return !em_.get_entry_by_name(n)->is_ros1_; }); + + return plugin_lifecycle_mgr_->shutdown(service_timeout_, call_timeout_, false, ros2_names).empty(); + } + + void PluginManager::get_registered_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) + { + // convert to plugin list + for(const auto& plugin : em_.get_entries()) + { + carma_planning_msgs::msg::Plugin msg; + msg.activated = plugin.active_; + msg.available = plugin.available_; + msg.name = plugin.name_; + msg.type = plugin.type_; + msg.capability = plugin.capability_; + res->plugins.push_back(msg); + } + } + + void PluginManager::get_active_plugins(SrvHeader, carma_planning_msgs::srv::PluginList::Request::SharedPtr, carma_planning_msgs::srv::PluginList::Response::SharedPtr res) + { + // convert to plugin list + for(const auto& plugin : em_.get_entries()) + { + if(plugin.active_) + { + carma_planning_msgs::msg::Plugin msg; + msg.activated = plugin.active_; + msg.available = plugin.available_; + msg.name = plugin.name_; + msg.type = plugin.type_; + msg.capability = plugin.capability_; + res->plugins.push_back(msg); + } + } + } + + void PluginManager::activate_plugin(SrvHeader, carma_planning_msgs::srv::PluginActivation::Request::SharedPtr req, carma_planning_msgs::srv::PluginActivation::Response::SharedPtr res) + { + boost::optional requested_plugin = em_.get_entry_by_name(req->plugin_name); + + if(!requested_plugin) // If not a known plugin then obviously not activated. Though really it would be better to have an indication of name failure in the message + { + res->newstate = false; + return; + } + + if (requested_plugin->is_ros1_) + { + res->newstate = true; + return; + } + + bool activated = false; + if (req->activated) + { + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, requested_plugin->name_, service_timeout_, call_timeout_); + + activated = (result_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } else { + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, requested_plugin->name_, service_timeout_, call_timeout_); + + activated = (result_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + + + Entry updated_entry(requested_plugin->available_, activated, requested_plugin->name_, requested_plugin->type_, requested_plugin->capability_, true); // Mark as user activated + em_.update_entry(updated_entry); + + res->newstate = activated; + } + + void PluginManager::update_plugin_status(carma_planning_msgs::msg::Plugin::UniquePtr msg) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "received status from: " << msg->name); + boost::optional requested_plugin = em_.get_entry_by_name(msg->name); + + if (!requested_plugin) // This is a new plugin so we need to add it + { + Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, false); + add_plugin(plugin); + return; + } + + Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, requested_plugin->user_requested_activation_); + + em_.update_entry(plugin); + } + + bool PluginManager::matching_capability(const std::vector& base_capability_levels, const std::vector& compared_capability_levels) + { + for (size_t i=0; i < base_capability_levels.size() && i < compared_capability_levels.size(); i++) + { + if (compared_capability_levels[i].compare(base_capability_levels[i]) != 0) + return false; + } + + return true; + } + + void PluginManager::get_control_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) + { + std::vector req_capability_levels; + boost::split(req_capability_levels, req->capability, boost::is_any_of("/")); + + for(const auto& plugin : em_.get_entries()) + { + + std::vector plugin_capability_levels; + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")); + + if(plugin.type_ == carma_planning_msgs::msg::Plugin::CONTROL && + (req->capability.size() == 0 || (matching_capability(plugin_capability_levels, req_capability_levels) && plugin.active_ && plugin.available_))) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "discovered control plugin: " << plugin.name_); + res->plan_service.push_back(plugin.name_ + control_trajectory_suffix_); + } + else + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "not valid control plugin: " << plugin.name_); + } + } + } + + void PluginManager::get_tactical_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) + { + std::vector req_capability_levels; + boost::split(req_capability_levels, req->capability, boost::is_any_of("/")); + + for(const auto& plugin : em_.get_entries()) + { + + std::vector plugin_capability_levels; + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")); + + if(plugin.type_ == carma_planning_msgs::msg::Plugin::TACTICAL && + (req->capability.size() == 0 || (matching_capability(plugin_capability_levels, req_capability_levels) && plugin.active_ && plugin.available_))) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "discovered tactical plugin: " << plugin.name_); + res->plan_service.push_back(plugin.name_ + plan_trajectory_suffix_); + } + else + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "not valid tactical plugin: " << plugin.name_); + } + } + } + + void PluginManager::get_strategic_plugins_by_capability(SrvHeader, carma_planning_msgs::srv::GetPluginApi::Request::SharedPtr req, carma_planning_msgs::srv::GetPluginApi::Response::SharedPtr res) + { + std::vector req_capability_levels; + boost::split(req_capability_levels, req->capability, boost::is_any_of("/")); + + for(const auto& plugin : em_.get_entries()) + { + + std::vector plugin_capability_levels; + boost::split(plugin_capability_levels, plugin.capability_, boost::is_any_of("/")); + + if(plugin.type_ == carma_planning_msgs::msg::Plugin::STRATEGIC && + (req->capability.size() == 0 || (matching_capability(plugin_capability_levels, req_capability_levels) && plugin.active_ && plugin.available_))) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "discovered strategic plugin: " << plugin.name_); + res->plan_service.push_back(plugin.name_ + plan_maneuvers_suffix_); + } + else + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "not valid strategic plugin: " << plugin.name_); + } + } + } + +} diff --git a/subsystem_controllers/test/CMakeLists.txt b/subsystem_controllers/test/CMakeLists.txt index fed79e44ef..8762036ded 100644 --- a/subsystem_controllers/test/CMakeLists.txt +++ b/subsystem_controllers/test/CMakeLists.txt @@ -22,10 +22,12 @@ add_executable(test_carma_lifecycle_node ament_add_gtest(controllers_gtest localization_controller_test.cpp + test_plugin_manager.cpp ) target_link_libraries(controllers_gtest localization_controller_core + guidance_controller_core ) ament_target_dependencies(test_carma_lifecycle_node ${dependencies} ) diff --git a/subsystem_controllers/test/test_entry_manager.cpp b/subsystem_controllers/test/test_entry_manager.cpp new file mode 100644 index 0000000000..b76f093572 --- /dev/null +++ b/subsystem_controllers/test/test_entry_manager.cpp @@ -0,0 +1,162 @@ +/* + * 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_manager.h" +#include +#include + +namespace subsystem_controllers +{ + + TEST(EntryManagerTest, testAddNewEntry) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin", 2000, 3, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + 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) + { + EntryManager em; + // params: bool available, bool active, std::string name, long timestamp, uint8_t type + Entry entry(true, false, "cruising_plugin", 1000, 1, ""); + em.update_entry(entry); + boost::optional res = em.get_entry_by_name("plugin"); + if(!res) + { + ASSERT_TRUE(1 == 1); + } else + { + ASSERT_TRUE(1 == 2); + } + } + + TEST(EntryManagerTest, testRequiredEntryCheck) + { + 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, ""); + em.update_entry(entry); + Entry new_entry(false, true, "cruising_plugin_2", 2000, 3, ""); + 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")); + } + +} diff --git a/subsystem_controllers/test/test_plugin_manager.cpp b/subsystem_controllers/test/test_plugin_manager.cpp new file mode 100644 index 0000000000..7a7b1ea1cb --- /dev/null +++ b/subsystem_controllers/test/test_plugin_manager.cpp @@ -0,0 +1,380 @@ +/* + * Copyright (C) 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 + +#include + +#include "subsystem_controllers/guidance_controller/plugin_manager.h" +#include + +using std_msec = std::chrono::milliseconds; +using std_nanosec = std::chrono::nanoseconds; + +namespace subsystem_controllers +{ + + uint8_t UNKOWN = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; + uint8_t UNCONFIGURED = lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; + uint8_t INACTIVE = lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; + uint8_t ACTIVE = lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + uint8_t FINALIZED = lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED; + + + class MockLifecycleManager : public ros2_lifecycle_manager::LifecycleManagerInterface + { + public: + + std::unordered_map node_states; + + virtual ~MockLifecycleManager(){}; + + void set_managed_nodes(const std::vector &nodes) + { + for (auto n : nodes) + { + node_states.insert({n, UNKOWN}); + } + } + + void add_managed_node(const std::string& node){ node_states.insert({node, UNKOWN}); } + + std::vector get_managed_nodes() + { + std::vector keys; + for(auto const& n: node_states) + keys.push_back(n.first); + + return keys; + } + + uint8_t get_managed_node_state(const std::string &node) { return node_states.at(node); }; + + + uint8_t transition_node_to_state(const uint8_t state, const std::string& node, const std_nanosec &, const std_nanosec &) + { + node_states[node] = state; + return state; + } + + std::vector simple_transition(uint8_t state, std::vector nodes = {}) + { + if (nodes.empty()) + { + for (auto n : node_states) + { + transition_node_to_state(state, n.first, std::chrono::milliseconds(10), std::chrono::milliseconds(10)); + } + + return {}; + } + + for (auto n : nodes) + { + transition_node_to_state(state, n, std::chrono::milliseconds(10), std::chrono::milliseconds(10)); + } + + return {}; + } + + std::vector configure(const std_nanosec &, const std_nanosec &, bool, std::vector nodes) + { + return simple_transition(INACTIVE, nodes); + }; + + std::vector cleanup(const std_nanosec &, const std_nanosec &, bool, std::vector nodes) + { + return simple_transition(UNCONFIGURED, nodes); + } + + std::vector activate(const std_nanosec &, const std_nanosec &, bool, std::vector nodes) + { + return simple_transition(ACTIVE, nodes); + } + + std::vector deactivate(const std_nanosec &, const std_nanosec &, bool, std::vector nodes) + { + return simple_transition(INACTIVE, nodes); + } + + std::vector shutdown(const std_nanosec &, const std_nanosec &, bool, std::vector nodes) + { + return simple_transition(FINALIZED, nodes); + } + + }; + + TEST(plugin_manager_test, transitions_test) + { + std::vector req_plugins = {"plg_1"}; + std::vector auto_actv_plugins = {"plg_2"}; + + auto mlm = std::make_shared(); + + uint8_t curr_state = UNCONFIGURED; + + PluginManager pm( + req_plugins, + auto_actv_plugins, + mlm, + [&curr_state](){ return curr_state; }, + [](auto node, auto) -> std::map>> { + if (node=="plg_1") + { + return { {"plg_1/change_state", { "lifecycle_msgs/srv/ChangeState" } }, + {"plg_1/get_state", { "lifecycle_msgs/srv/GetState" } } }; + } else if (node == "plg_2") + { + return { {"plg_2/change_state", {"lifecycle_msgs/srv/ChangeState"}}, + {"plg_2/get_state", {"lifecycle_msgs/srv/GetState"}} }; + } + else if (node == "plg_3") + { + return { {"plg_3/change_state", {"lifecycle_msgs/srv/ChangeState"}}, + {"plg_3/get_state", {"lifecycle_msgs/srv/GetState"}} }; + } else + { + return {}; + } + }, + std_msec(100), std_msec(100) + ); + + ///// CONFIGURE ///// + curr_state=INACTIVE; + ASSERT_TRUE(pm.configure()); + + ASSERT_EQ(mlm->get_managed_node_state("plg_1"), INACTIVE); + ASSERT_EQ(mlm->get_managed_node_state("plg_2"), INACTIVE); + + SrvHeader hdr; + + auto req = std::make_shared(); + auto res = std::make_shared(); + + pm.get_registered_plugins(hdr, req, res); + + ASSERT_EQ(res->plugins.size(), 2ul); + + bool has_p1 = false; + bool has_p2 = false; + + for(auto p : res->plugins) + { + if (p.name == "plg_1") + has_p1 = true; + if (p.name == "plg_2") + has_p2 = true; + } + + ASSERT_TRUE(has_p1); + ASSERT_TRUE(has_p2); + + res = std::make_shared(); + pm.get_active_plugins(hdr, req, res); + + ASSERT_TRUE(res->plugins.empty()); + + //// ACTIVATE //// + ASSERT_TRUE(pm.activate()); + curr_state=ACTIVE; + + ASSERT_EQ(mlm->get_managed_node_state("plg_1"), ACTIVE); + ASSERT_EQ(mlm->get_managed_node_state("plg_2"), ACTIVE); + + res = std::make_shared(); + pm.get_active_plugins(hdr, req, res); + + has_p1 = false; + has_p2 = false; + + for(auto p : res->plugins) + { + if (p.name == "plg_1") + has_p1 = true; + if (p.name == "plg_2") + has_p2 = true; + } + + ASSERT_TRUE(has_p1); + ASSERT_TRUE(has_p2); + + auto api_req = std::make_shared(); + auto api_res = std::make_shared(); + + // We have not yet send plugin discovery messages so the type and capabilities of the activated plugins is still unknown + api_req->capability = "strategic_plan/plan_maneuvers"; + api_res = std::make_shared(); + pm.get_strategic_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_TRUE(api_res->plan_service.empty()); + + api_req->capability = "tactical_plan/plan_trajectory"; + api_res = std::make_shared(); + pm.get_tactical_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_TRUE(api_res->plan_service.empty()); + + api_req->capability = "control/trajectory_control"; + api_res = std::make_shared(); + pm.get_control_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_TRUE(api_res->plan_service.empty()); + + // Add discovery messages for the two current plugins + + auto plugin_1_stat = std::make_unique(); + plugin_1_stat->name = "plg_1"; + plugin_1_stat->version_id = "v1"; + plugin_1_stat->type = carma_planning_msgs::msg::Plugin::STRATEGIC; + plugin_1_stat->available = true; + plugin_1_stat->activated = true; + plugin_1_stat->capability = "strategic_plan/plan_maneuvers"; + pm.update_plugin_status(std::move(plugin_1_stat)); + + auto plugin_2_stat = std::make_unique(); + plugin_2_stat->name = "plg_2"; + plugin_2_stat->version_id = "v1"; + plugin_2_stat->type = carma_planning_msgs::msg::Plugin::TACTICAL; + plugin_2_stat->available = true; + plugin_2_stat->activated = true; + plugin_2_stat->capability = "tactical_plan/plan_trajectory"; + pm.update_plugin_status(std::move(plugin_2_stat)); + + api_req->capability = "strategic_plan/plan_maneuvers"; + api_res = std::make_shared(); + pm.get_strategic_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_EQ(api_res->plan_service.size(), 1ul); + + ASSERT_TRUE(api_res->plan_service[0] == "plg_1/plan_maneuvers"); + + api_req->capability = "tactical_plan/plan_trajectory"; + api_res = std::make_shared(); + pm.get_tactical_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_EQ(api_res->plan_service.size(), 1ul); + + ASSERT_TRUE(api_res->plan_service[0] == "plg_2/plan_trajectory"); + + api_req->capability = "control/trajectory_control"; + api_res = std::make_shared(); + pm.get_control_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_TRUE(api_res->plan_service.empty()); + + // Add a controller plugin + auto plugin_3_stat = std::make_unique(); + plugin_3_stat->name = "plg_3"; + plugin_3_stat->version_id = "v1"; + plugin_3_stat->type = carma_planning_msgs::msg::Plugin::CONTROL; + plugin_3_stat->available = true; + plugin_3_stat->activated = true; + plugin_3_stat->capability = "control/trajectory_control"; + pm.update_plugin_status(std::move(plugin_3_stat)); + + api_req->capability = "strategic_plan/plan_maneuvers"; + api_res = std::make_shared(); + pm.get_strategic_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_EQ(api_res->plan_service.size(), 1ul); + + ASSERT_TRUE(api_res->plan_service[0] == "plg_1/plan_maneuvers"); + + api_req->capability = "tactical_plan/plan_trajectory"; + api_res = std::make_shared(); + pm.get_tactical_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_EQ(api_res->plan_service.size(), 1ul); + + ASSERT_TRUE(api_res->plan_service[0] == "plg_2/plan_trajectory"); + + // ACTIVATE plugin 3 + auto activate_req = std::make_shared(); + auto activate_res = std::make_shared(); + activate_req->plugin_name = "plg_3"; + activate_req->plugin_version = "v1"; + activate_req->activated = true; + pm.activate_plugin(hdr, activate_req, activate_res); + + ASSERT_TRUE(activate_res->newstate); + + api_req->capability = "control/trajectory_control"; + api_res = std::make_shared(); + pm.get_control_plugins_by_capability(hdr, api_req, api_res); + + ASSERT_EQ(api_res->plan_service.size(), 1ul); + + ASSERT_TRUE(api_res->plan_service[0] == "plg_3/plan_trajectory"); + + res = std::make_shared(); + pm.get_active_plugins(hdr, req, res); + + has_p1 = false; + has_p2 = false; + bool has_p3 = false; + + for(auto p : res->plugins) + { + if (p.name == "plg_1") + has_p1 = true; + if (p.name == "plg_2") + has_p2 = true; + if (p.name == "plg_3") + has_p3 = true; + } + + ASSERT_TRUE(has_p1); + ASSERT_TRUE(has_p2); + ASSERT_TRUE(has_p3); + + // Deactivate plugin 3 + activate_res = std::make_shared(); + activate_req->activated = false; + pm.activate_plugin(hdr, activate_req, activate_res); + + ASSERT_FALSE(activate_res->newstate); + + res = std::make_shared(); + pm.get_active_plugins(hdr, req, res); + + has_p1 = false; + has_p2 = false; + has_p3 = false; + + for(auto p : res->plugins) + { + if (p.name == "plg_1") + has_p1 = true; + if (p.name == "plg_2") + has_p2 = true; + if (p.name == "plg_3") + has_p3 = true; + } + + ASSERT_TRUE(has_p1); + ASSERT_TRUE(has_p2); + ASSERT_FALSE(has_p3); + + } + +} diff --git a/template_package/carma_package b/template_package/carma_package index 12cd4d0940..63948930f6 100755 --- a/template_package/carma_package +++ b/template_package/carma_package @@ -111,7 +111,6 @@ if [ ! "$plugin_type" = "control" ]; then fi - # Perform substitutions find . -type f -exec sed -i "s//${package_name}/g" {} \; find . -type f -exec sed -i "s//${current_year}/g" {} \; diff --git a/template_package/include/template_package/template_control_plugin_node.hpp b/template_package/include/template_package/template_control_plugin_node.hpp index 8ded54b871..fdd5c3f84a 100644 --- a/template_package/include/template_package/template_control_plugin_node.hpp +++ b/template_package/include/template_package/template_control_plugin_node.hpp @@ -89,8 +89,6 @@ namespace bool get_availability() override; - std::string get_plugin_name() override; - std::string get_version_id() override; /** diff --git a/template_package/include/template_package/template_strategic_plugin_node.hpp b/template_package/include/template_package/template_strategic_plugin_node.hpp index 4648115165..abaf36a7b6 100644 --- a/template_package/include/template_package/template_strategic_plugin_node.hpp +++ b/template_package/include/template_package/template_strategic_plugin_node.hpp @@ -92,8 +92,6 @@ namespace bool get_availability() override; - std::string get_plugin_name() override; - std::string get_version_id() override; /** diff --git a/template_package/include/template_package/template_tactical_plugin_node.hpp b/template_package/include/template_package/template_tactical_plugin_node.hpp index 546a5bef27..4c9c1b8780 100644 --- a/template_package/include/template_package/template_tactical_plugin_node.hpp +++ b/template_package/include/template_package/template_tactical_plugin_node.hpp @@ -92,8 +92,6 @@ namespace bool get_availability() override; - std::string get_plugin_name() override; - std::string get_version_id() override; /** diff --git a/template_package/src/template_control_plugin_node.cpp b/template_package/src/template_control_plugin_node.cpp index 2a3ad09bf4..f45a88df6e 100644 --- a/template_package/src/template_control_plugin_node.cpp +++ b/template_package/src/template_control_plugin_node.cpp @@ -110,10 +110,6 @@ namespace return true; // TODO for user implement actual check on availability if applicable to plugin } - std::string Node::get_plugin_name() { - return "TODO for user specify plugin name here"; - } - std::string Node::get_version_id() { return "TODO for user specify plugin version id here"; } diff --git a/template_package/src/template_strategic_plugin_node.cpp b/template_package/src/template_strategic_plugin_node.cpp index cd2d288c48..c2299a79e4 100644 --- a/template_package/src/template_strategic_plugin_node.cpp +++ b/template_package/src/template_strategic_plugin_node.cpp @@ -111,10 +111,6 @@ namespace return true; // TODO for user implement actual check on availability if applicable to plugin } - std::string Node::get_plugin_name() { - return "TODO for user specify plugin name here"; - } - std::string Node::get_version_id() { return "TODO for user specify plugin version id here"; } diff --git a/template_package/src/template_tactical_plugin_node.cpp b/template_package/src/template_tactical_plugin_node.cpp index 7eb48f16ed..e02ad3e4db 100644 --- a/template_package/src/template_tactical_plugin_node.cpp +++ b/template_package/src/template_tactical_plugin_node.cpp @@ -111,10 +111,6 @@ namespace return true; // TODO for user implement actual check on availability if applicable to plugin } - std::string Node::get_plugin_name() { - return "TODO for user specify plugin name here"; - } - std::string Node::get_version_id() { return "TODO for user specify plugin version id here"; } diff --git a/trajectory_executor/config/parameters.yaml b/trajectory_executor/config/parameters.yaml index 7b4d6e86d1..992ce8cd7d 100644 --- a/trajectory_executor/config/parameters.yaml +++ b/trajectory_executor/config/parameters.yaml @@ -7,7 +7,7 @@ trajectory_publish_rate: 10.0 # Due to lack of existing plugin discovery mechanism this is the only control plugin # which will be available to TrajectoryExecutor # Units: N/a -default_control_plugin: "Pure Pursuit" +default_control_plugin: "pure_pursuit_wrapper_node" # String: Full path to default control plugin's trajectory input topic # Units: N/a diff --git a/trajectory_executor/include/trajectory_executor/trajectory_executor_config.hpp b/trajectory_executor/include/trajectory_executor/trajectory_executor_config.hpp index d1f5ad012a..80a44fba6d 100644 --- a/trajectory_executor/include/trajectory_executor/trajectory_executor_config.hpp +++ b/trajectory_executor/include/trajectory_executor/trajectory_executor_config.hpp @@ -26,7 +26,7 @@ namespace trajectory_executor { double trajectory_publish_rate = 10.0; // Publish rate (in Hz) for the outbound trajectories to the control plugins - std::string default_control_plugin = "Pure Pursuit"; // Name of default control plugin + std::string default_control_plugin = "pure_pursuit_wrapper_node"; // Name of default control plugin std::string default_control_plugin_topic = "/guidance/pure_pursuit/plan_trajectory"; // Full path to default control plugin's trajectory input topic diff --git a/trajectory_executor/src/trajectory_executor_node.cpp b/trajectory_executor/src/trajectory_executor_node.cpp index efe142a051..f549f18eb7 100644 --- a/trajectory_executor/src/trajectory_executor_node.cpp +++ b/trajectory_executor/src/trajectory_executor_node.cpp @@ -55,8 +55,8 @@ namespace trajectory_executor out[config_.default_control_plugin] = config_.default_control_plugin_topic; //Hardcoding platoon control plugins - std::string control_plugin2 = "PlatooningControlPlugin"; - std::string control_plugin_topic2 = "/guidance/PlatooningControlPlugin/plan_trajectory"; + std::string control_plugin2 = "platoon_control"; + std::string control_plugin_topic2 = "/guidance/platoon_control/plan_trajectory"; out[control_plugin2] = control_plugin_topic2; return out; diff --git a/trajectory_executor/test/trajectory_executor_test_suite.cpp b/trajectory_executor/test/trajectory_executor_test_suite.cpp index 53a48371aa..ddcd76b5af 100644 --- a/trajectory_executor/test/trajectory_executor_test_suite.cpp +++ b/trajectory_executor/test/trajectory_executor_test_suite.cpp @@ -65,7 +65,7 @@ namespace trajectory_executor_test_suite // Setup Subscribers traj_sub_ = create_subscription("/guidance/pure_pursuit/plan_trajectory", 100, std::bind(&TrajectoryExecutorTestSuite::trajEmitCallback, this, std_ph::_1)); - traj_sub2_ = create_subscription("/guidance/PlatooningControlPlugin/plan_trajectory", 100, + traj_sub2_ = create_subscription("/guidance/platoon_control/plan_trajectory", 100, std::bind(&TrajectoryExecutorTestSuite::trajEmitCallback, this, std_ph::_1)); return CallbackReturn::SUCCESS; @@ -88,7 +88,7 @@ namespace trajectory_executor_test_suite rclcpp::Time cur_time = rclcpp::Time(0,0); for (int i = 0; i < 10; i++) { carma_planning_msgs::msg::TrajectoryPlanPoint p; - p.controller_plugin_name = "Pure Pursuit"; + p.controller_plugin_name = "pure_pursuit_wrapper_node"; p.lane_id = "0"; p.planner_plugin_name = "cruising"; rclcpp::Duration dur((i * 0.13)*1e9); // Convert seconds to nanoseconds @@ -116,7 +116,7 @@ namespace trajectory_executor_test_suite rclcpp::Time cur_time = rclcpp::Time(0,0); for (int i = 0; i < 10; i++) { carma_planning_msgs::msg::TrajectoryPlanPoint p; - p.controller_plugin_name = "PlatooningControlPlugin"; + p.controller_plugin_name = "platoon_control"; p.lane_id = "0"; p.planner_plugin_name = "cruising"; rclcpp::Duration dur((i * 0.13)*1e9); // Convert seconds to nanoseconds diff --git a/unobstructed_lanechange/CMakeLists.txt b/unobstructed_lanechange/CMakeLists.txt deleted file mode 100644 index 79290e911a..0000000000 --- a/unobstructed_lanechange/CMakeLists.txt +++ /dev/null @@ -1,133 +0,0 @@ -# Copyright (C) 2018-2020 LEIDOS. -# -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -cmake_minimum_required(VERSION 2.8.3) -project(unobstructed_lanechange) - -find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) -carma_package() - -set(CATKIN_DEPS - roscpp - std_msgs - cav_msgs - cav_srvs - carma_utils - trajectory_utils - autoware_msgs - carma_wm - tf - tf2 - tf2_geometry_msgs - basic_autonomy - roslib -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) -find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### - - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost EIGEN3 -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -file(GLOB_RECURSE headers */*.hpp */*.h) - -add_executable(${PROJECT_NAME} - src/main.cpp) - - -add_library(unobstructed_lanechange_plugin_lib -src/unobstructed_lanechange.cpp -) - -add_dependencies(unobstructed_lanechange_plugin_lib ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(unobstructed_lanechange_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -target_link_libraries(${PROJECT_NAME} unobstructed_lanechange_plugin_lib) - -add_library(lc_helper test/helper.cpp) -target_link_libraries(lc_helper - ${catkin_LIBRARIES} -) - -add_executable(lc_helper_node test/helper.cpp) -target_link_libraries(lc_helper_node - lc_helper - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - - -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Install C++ -install(TARGETS ${PROJECT_NAME} unobstructed_lanechange_plugin_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - - -############# -## Testing ## -############# -catkin_add_gmock(${PROJECT_NAME}-test - test/test_unobstructed_lanechange_plugin.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) -target_link_libraries(${PROJECT_NAME}-test unobstructed_lanechange_plugin_lib ${catkin_LIBRARIES}) - -# if(CATKIN_ENABLE_TESTING) -# find_package(rostest REQUIRED) -# add_rostest_gtest(rostest_ulc test/ulc_plugin.test test/rostest_ulc.cpp) -# target_link_libraries(rostest_ulc ${catkin_LIBRARIES}) -# endif() \ No newline at end of file diff --git a/unobstructed_lanechange/config/parameters.yaml b/unobstructed_lanechange/config/parameters.yaml deleted file mode 100644 index 5f5720bebb..0000000000 --- a/unobstructed_lanechange/config/parameters.yaml +++ /dev/null @@ -1,81 +0,0 @@ -# Copyright (C) 2019-2020 LEIDOS. - -# Licensed under the Apache License, Version 2.0 (the "License"); you may not -# use this file except in compliance with the License. You may obtain a copy of -# the License at - -# http://www.apache.org/licenses/LICENSE-2.0 - -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -# License for the specific language governing permissions and limitations under -# the License. - -# Double: The length of the trajectory in time domain, in seconds -# Units: Seconds -trajectory_time_length: 6.0 - -# String: The default control plugin name -control_plugin_name: "mpc_follower" - - -# Double : Minimum allowable speed -# Units : m/s -minimum_speed : 2.2352 - -# Double : Maximum allowable longitudinal acceleration -# Units : m/s^2 -max_accel : 1.5 - -# Double : Minimum value for lookahead distance -# Units : meters -minimum_lookahead_distance : 5.0 - -# Double : Maximum value for lookahead distance -# Units : meters -maximum_lookahead_distance : 25.0 - -# Double : Minimum speed value for lookahead calculation -# Units : m/s -minimum_lookahead_speed : 2.8 - -# Double : Maximum speed value for lookahead calculation -# Units : m/s -maximum_lookahead_speed : 13.9 - -# Double : Maximum allowable lateral acceleration -# Units : m/s^2 -lateral_accel_limit : 1.5 - -# Int : Size of the window used in the moving average filter to smooth both the computed curvature and output speeds -speed_moving_average_window_size : 5 - -#Int: // Size of the window used in the moving average filter to smooth the curvature profile -curvature_moving_average_window_size : 9 - -# Int : Number of points to look ahead when calculating the curvature of the lanelet centerline -curvature_calc_lookahead_count : 1 - -# Int: Amount to downsample input lanelet centerline data. Value corresponds to saving each nth point. -downsample_ratio : 8 - -# bool: Flag to activate and deactivate object avoidance communication with Yield -enable_object_avoidance_lc : false - -# Double: The minimum timestep used for planning trajectory -#Units: seconds -min_timestep : 0.1 - -#Int: Amount to downsample input lanelet centerline data on turns. Value corresponds to saving each nth point. -turn_downsample_ratio: 0 - -#Double: Curve re-sampling step size in m -curve_resample_step_size: 1.0 - -#Double: Number of meters behind the first maneuver that need to be included in points for curvature calculation -back_distance: 0.0 - -# Double : The additional downtrack beyond requested end dist used to fit points along spline. Extra points removed before returning trajectory. Feature not used if end dist is route end -#Units: meters -ending_buffer_downtrack : 5.0 diff --git a/unobstructed_lanechange/include/unobstructed_lanechange.h b/unobstructed_lanechange/include/unobstructed_lanechange.h deleted file mode 100644 index 3db4dde8b3..0000000000 --- a/unobstructed_lanechange/include/unobstructed_lanechange.h +++ /dev/null @@ -1,164 +0,0 @@ -#pragma once -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - - -namespace unobstructed_lanechange -{ - /** - * \brief Convenience class for pairing 2d points with speeds - */ - struct PointSpeedPair - { - lanelet::BasicPoint2d point; - double speed = 0; - }; - - class UnobstructedLaneChangePlugin - { - public: - /** - * \brief General entry point to begin the operation of this class - */ - void run(); - - /** - * \brief Service callback for trajectory planning - * - * \param req The service request - * \param resp The service response - * - * \return True if success. False otherwise - */ - bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest &req, cav_srvs::PlanTrajectoryResponse &resp); - - /** - * \brief verify if the input yield trajectory plan is valid - * - * \param yield_plan input yield trajectory plan - * - * \return true or falss - */ - bool validate_yield_plan(const cav_msgs::TrajectoryPlan& yield_plan, const std::string& original_plan_id); - - //Internal Variables used in unit tests - // Current vehicle forward speed - double current_speed_; - - // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; - - // wm listener pointer and pointer to the actual wm object - std::shared_ptr wml_; - carma_wm::WorldModelConstPtr wm_; - - private: - - // node handles - std::shared_ptr nh_, pnh_; - - ros::Publisher unobstructed_lanechange_plugin_discovery_pub_; - - // ros service servers - ros::ServiceServer trajectory_srv_; - ros::ServiceServer maneuver_srv_; - - // ros service client - ros::ServiceClient yield_client_; - - // ROS publishers and subscribers - cav_msgs::Plugin plugin_discovery_msg_; - ros::Subscriber pose_sub_; - ros::Subscriber twist_sub_; - ros::Timer discovery_pub_timer_; - - // trajectory frequency - double traj_freq = 10; - - //fraction of the maneuver completed - double maneuver_fraction_completed_ = 0; - - // ROS params - double trajectory_time_length_ = 6; - std::string control_plugin_name_ = "mpc_follower"; - double minimum_speed_ = 2.0; - double max_accel_ = 1.5; - double minimum_lookahead_distance_ = 5.0; - double maximum_lookahead_distance_ = 25.0; - double minimum_lookahead_speed_ = 2.8; - double maximum_lookahead_speed_ =13.9; - double lateral_accel_limit_ = 1.5; - int speed_moving_average_window_size_ = 5; - int curvature_moving_average_window_size_ = 9; - double curvature_calc_lookahead_count_ = 1; - int downsample_ratio_ =8; - bool enable_object_avoidance_lc_ = false; - double min_timestep_ = 0.1; - int turn_downsample_ratio_ = 0.0; - double curve_resample_step_size_ = 0.0; - double back_distance_ = 0.0; - double buffer_ending_downtrack_ = 0.0; - - cav_msgs::VehicleState ending_state_before_buffer_; - - // Time duration to ensure plan is recent - double acceptable_time_difference_ = 1.0; - ros::Duration time_dur_ = ros::Duration(acceptable_time_difference_); - - int num_points = traj_freq * trajectory_time_length_; - - - // generated trajectory plan - cav_msgs::TrajectoryPlan trajectory_msg; - - // initialize this node - void initialize(); - - /** - * \brief Callback for the pose subscriber, which will store latest pose locally - * \param msg Latest pose message - */ - void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); - - /** - * \brief Callback for the twist subscriber, which will store latest twist locally - * \param msg Latest twist message - */ - void twist_cd(const geometry_msgs::TwistStampedConstPtr& msg); - - - - }; -} \ No newline at end of file diff --git a/unobstructed_lanechange/launch/unobstructed_lanechange.launch b/unobstructed_lanechange/launch/unobstructed_lanechange.launch deleted file mode 100644 index 6c191bbc24..0000000000 --- a/unobstructed_lanechange/launch/unobstructed_lanechange.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - diff --git a/unobstructed_lanechange/package.xml b/unobstructed_lanechange/package.xml deleted file mode 100644 index 38b48ee7d0..0000000000 --- a/unobstructed_lanechange/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - - unobstructed_lanechange - 3.3.0 - The node is to plan an unobstructed lane change trajectory - carma - Apache 2.0 License - catkin - roscpp - std_msgs - cav_srvs - cav_msgs - carma_utils - autoware_msgs - carma_wm - tf - tf2 - tf2_geometry_msgs - trajectory_utils - basic_autonomy - roslib - carma_cmake_common - diff --git a/unobstructed_lanechange/src/main.cpp b/unobstructed_lanechange/src/main.cpp deleted file mode 100644 index 0b20c9c149..0000000000 --- a/unobstructed_lanechange/src/main.cpp +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - -#include -#include "unobstructed_lanechange.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "unobstructed_lanechange"); - unobstructed_lanechange::UnobstructedLaneChangePlugin node; - node.run(); - return 0; -}; \ No newline at end of file diff --git a/unobstructed_lanechange/src/unobstructed_lanechange.cpp b/unobstructed_lanechange/src/unobstructed_lanechange.cpp deleted file mode 100644 index 8007aa50e4..0000000000 --- a/unobstructed_lanechange/src/unobstructed_lanechange.cpp +++ /dev/null @@ -1,213 +0,0 @@ -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ -#include -#include -#include -#include -#include "unobstructed_lanechange.h" -#include -#include -#include -#include -#include -#include - -namespace unobstructed_lanechange -{ - void UnobstructedLaneChangePlugin::initialize() - { - nh_.reset(new ros::CARMANodeHandle()); - pnh_.reset(new ros::CARMANodeHandle("~")); - - trajectory_srv_ = nh_->advertiseService("plugins/UnobstructedLaneChangePlugin/plan_trajectory", &UnobstructedLaneChangePlugin::plan_trajectory_cb, this); - yield_client_ = nh_->serviceClient("plugins/YieldPlugin/plan_trajectory"); - - unobstructed_lanechange_plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); - plugin_discovery_msg_.name = "UnobstructedLaneChangePlugin"; - plugin_discovery_msg_.version_id = "v1.0"; - plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; - plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; - plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; - - pose_sub_ = nh_->subscribe("current_pose", 1, &UnobstructedLaneChangePlugin::pose_cb, this); - twist_sub_ = nh_->subscribe("current_velocity", 1, &UnobstructedLaneChangePlugin::twist_cd, this); - - pnh_->param("trajectory_time_length", trajectory_time_length_, 6.0); - pnh_->param("control_plugin_name", control_plugin_name_, "NULL"); - pnh_->param("minimum_speed", minimum_speed_, 2.2352); - pnh_->param("max_accel", max_accel_, 1.5); - pnh_->param("minimum_lookahead_distance", minimum_lookahead_distance_, 5.0); - pnh_->param("maximum_lookahead_distance", maximum_lookahead_distance_, 25.0); - pnh_->param("minimum_lookahead_speed", minimum_lookahead_speed_, 2.8); - pnh_->param("maximum_lookahead_speed", maximum_lookahead_speed_, 13.9); - pnh_->param("lateral_accel_limit", lateral_accel_limit_, 1.5); - pnh_->param("speed_moving_average_window_size", speed_moving_average_window_size_, 5); - pnh_->param("curvature_moving_average_window_size", curvature_moving_average_window_size_, 9); - pnh_->param("curvature_calc_lookahead_count", curvature_calc_lookahead_count_, 1); - pnh_->param("downsample_ratio", downsample_ratio_, 8); - pnh_->param("enable_object_avoidance_lc", enable_object_avoidance_lc_, false); - pnh_->param("acceptable_time_difference_", acceptable_time_difference_, 1.0); - pnh_->param("min_timestep",min_timestep_); - pnh_->param("turn_downsample_ratio", turn_downsample_ratio_, 0); - pnh_->param("curve_resample_step_size", curve_resample_step_size_, 0); - pnh_->param("back_distance", back_distance_, 0.0); - pnh_->param("buffer_ending_downtrack", buffer_ending_downtrack_, 0); - // update ros_dur_ value - time_dur_ = ros::Duration(acceptable_time_difference_); - - discovery_pub_timer_ = pnh_->createTimer( - ros::Duration(ros::Rate(10.0)), - [this](const auto&) { unobstructed_lanechange_plugin_discovery_pub_.publish(plugin_discovery_msg_); }); - - wml_.reset(new carma_wm::WMListener()); - wm_ = wml_->getWorldModel(); - - } - - void UnobstructedLaneChangePlugin::pose_cb(const geometry_msgs::PoseStampedConstPtr& msg) - { - pose_msg_ = geometry_msgs::PoseStamped(*msg.get()); - } - void UnobstructedLaneChangePlugin::twist_cd(const geometry_msgs::TwistStampedConstPtr& msg) - { - current_speed_ = msg->twist.linear.x; - } - - void UnobstructedLaneChangePlugin::run() - { - initialize(); - ros::CARMANodeHandle::spin(); - - } - - bool UnobstructedLaneChangePlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest &req, cav_srvs::PlanTrajectoryResponse &resp){ - - lanelet::BasicPoint2d veh_pos(req.vehicle_state.x_pos_global, req.vehicle_state.y_pos_global); - double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack; - - //convert maneuver info to route points and speeds - std::vector maneuver_plan; - if(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan].type != cav_msgs::Maneuver::LANE_CHANGE) - { - throw std::invalid_argument ("Unobstructed Lane Change Plugin doesnt support this maneuver type"); - } - maneuver_plan.push_back(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan]); - resp.related_maneuvers.push_back(req.maneuver_index_to_plan); - - basic_autonomy::waypoint_generation::DetailedTrajConfig wpg_detail_config; - basic_autonomy::waypoint_generation::GeneralTrajConfig wpg_general_config; - - wpg_general_config = basic_autonomy::waypoint_generation::compose_general_trajectory_config("unobstructed_lanechange", downsample_ratio_, turn_downsample_ratio_); - - wpg_detail_config = basic_autonomy::waypoint_generation::compose_detailed_trajectory_config(trajectory_time_length_, - curve_resample_step_size_, minimum_speed_, - max_accel_, lateral_accel_limit_, - speed_moving_average_window_size_, - curvature_moving_average_window_size_, back_distance_, - buffer_ending_downtrack_); - - ROS_DEBUG_STREAM("Current downtrack:"<= 2 && yield_plan.trajectory_id == original_plan_id) - { - ros::Duration time_difference = yield_plan.trajectory_points[0].target_time - ros::Time::now(); - - if (time_difference <= time_dur_) - { - return true; - } - else - { - ROS_DEBUG_STREAM("Old Yield Trajectory"); - } - } - else - { - ROS_DEBUG_STREAM("Invalid Yield Trajectory"); - } - return false; - } - -} \ No newline at end of file diff --git a/unobstructed_lanechange/test/TestHelpers.h b/unobstructed_lanechange/test/TestHelpers.h deleted file mode 100644 index da245f7102..0000000000 --- a/unobstructed_lanechange/test/TestHelpers.h +++ /dev/null @@ -1,162 +0,0 @@ -#pragma once -/* - * Copyright (C) 2019 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 - -/** - * 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); -} - -inline void addStraightRoute(CARMAWorldModel& cmw) -{ - // 1. Construct map - auto pl1 = getPoint(0, 0, 0); - auto pl2 = getPoint(0, 1, 0); - auto pl3 = getPoint(0, 2, 0); - auto pr1 = getPoint(1, 0, 0); - auto pr2 = getPoint(1, 1, 0); - auto pr3 = getPoint(1, 2, 0); - std::vector left_1 = { pl1, pl2 }; - std::vector right_1 = { pr1, pr2 }; - auto ll_1 = getLanelet(left_1, right_1); - - std::vector left_2 = { pl2, pl3 }; - std::vector right_2 = { pr2, pr3 }; - auto ll_2 = getLanelet(left_2, right_2); - - // 2. Build map but do not assign - // Create basic map and verify that the map and routing graph can be build, but the route remains false - lanelet::LaneletMapPtr map = lanelet::utils::createMap({ ll_1, ll_2 }, {}); - - // 3. Build routing graph but do not assign - // Build routing graph from map - lanelet::traffic_rules::TrafficRulesUPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::VehicleCar); - lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*map, *traffic_rules); - - // 4. Generate route - auto optional_route = map_graph->getRoute(ll_1, ll_2); - lanelet::routing::Route route = std::move(*optional_route); - LaneletRoutePtr route_ptr = std::make_shared(std::move(route)); - // 5. Set route and map - cmw.setRoute(route_ptr); - cmw.setMap(map); -} - -inline void addDisjointRoute(CARMAWorldModel& cmw) -{ - // 1. Construct map - auto p1 = getPoint(0, 0, 0); - auto p2 = getPoint(0, 1, 0); - auto p3 = getPoint(1, 1, 0); - auto p4 = getPoint(1, 2, 0); - auto p5 = getPoint(1, 0, 0); - auto p6 = getPoint(2, 0, 0); - auto p7 = getPoint(2, 1, 0); - auto p8 = getPoint(2, 2, 0); - lanelet::LineString3d left_ls_1(lanelet::utils::getId(), { p1, p2 }); - lanelet::LineString3d right_ls_1(lanelet::utils::getId(), { p5, p3 }); - auto ll_1 = getLanelet(left_ls_1, right_ls_1, lanelet::AttributeValueString::SolidSolid, - lanelet::AttributeValueString::Dashed); - - lanelet::LineString3d right_ls_2(lanelet::utils::getId(), { p6, p7 }); - auto ll_2 = - getLanelet(right_ls_1, right_ls_2, lanelet::AttributeValueString::Dashed, lanelet::AttributeValueString::Solid); - - lanelet::LineString3d left_ls_3(lanelet::utils::getId(), { p3, p4 }); - lanelet::LineString3d right_ls_3(lanelet::utils::getId(), { p7, p8 }); - auto ll_3 = - getLanelet(left_ls_3, right_ls_3, lanelet::AttributeValueString::Solid, lanelet::AttributeValueString::Solid); - - // 2. Build map but do not assign - // Create basic map and verify that the map and routing graph can be build, but the route remains false - lanelet::LaneletMapPtr map = lanelet::utils::createMap({ ll_1, ll_2, ll_3 }, {}); - - // 3. Build routing graph but do not assign - // Build routing graph from map - lanelet::traffic_rules::TrafficRulesUPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::VehicleCar); - lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*map, *traffic_rules); - - // 4. Generate route - auto optional_route = map_graph->getRoute(ll_1, ll_3); - // map_graph->exportGraphViz("my_graph"); // Uncomment to visualize route graph - lanelet::routing::Route route = std::move(*optional_route); - LaneletRoutePtr route_ptr = std::make_shared(std::move(route)); - // 5. Set route and map - cmw.setRoute(route_ptr); - cmw.setMap(map); -} -} // namespace carma_wm \ No newline at end of file diff --git a/unobstructed_lanechange/test/helper.cpp b/unobstructed_lanechange/test/helper.cpp deleted file mode 100644 index 8000797e81..0000000000 --- a/unobstructed_lanechange/test/helper.cpp +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - - -#include "ros/ros.h" -#include - -bool callback(cav_srvs::PlanTrajectory::Request &req, - cav_srvs::PlanTrajectory::Response &res) -{ - if (req.initial_trajectory_plan.trajectory_id == "YieldReq"){ - res.trajectory_plan.trajectory_id = "YieldResp"; - } - ROS_ERROR("Yield callback"); - return true; -} - -// Helper node to include the callback function for yield trajectory -int main(int argc, char **argv) -{ - ros::init(argc, argv, "helper"); - ros::NodeHandle n; - - ros::ServiceServer service = n.advertiseService("plugins/YieldPlugin/plan_trajectory", callback); - ros::spin(); - ros::Duration(5).sleep(); - - return 0; -} \ No newline at end of file diff --git a/unobstructed_lanechange/test/rostest_ulc.cpp b/unobstructed_lanechange/test/rostest_ulc.cpp deleted file mode 100644 index db4e86b268..0000000000 --- a/unobstructed_lanechange/test/rostest_ulc.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - -#include "unobstructed_lanechange.h" -#include -#include -#include -#include "ros/service.h" -#include -#include - -#include "ros/ros.h" -#include - -TEST(UnobstructedLaneChangePluginTest, rostest1) -{ - ros::CARMANodeHandle nh; - bool flag_trajectory = false; - bool flag_yield = false; - std::string res = ""; - - cav_srvs::PlanTrajectory traj_srv; - traj_srv.request.initial_trajectory_plan.trajectory_id = "LCReq"; - - ros::ServiceClient plugin1= nh.serviceClient("plugins/UnobstructedLaneChangePlugin/plan_trajectory"); - - ROS_INFO_STREAM("ilc service: " << plugin1.getService()); - if(plugin1.waitForExistence(ros::Duration(5.0))) - { - ros::spinOnce(); - ROS_ERROR("waiting"); - if (plugin1.call(traj_srv)) - { - res = traj_srv.response.trajectory_plan.trajectory_id; - ROS_ERROR("LC Traj Service called"); - flag_trajectory = true; - flag_yield = true; - - } - else - { - ROS_ERROR("ILC Trajectory Service not called"); - res = "error"; - } - } - EXPECT_TRUE(flag_trajectory); - ASSERT_TRUE(flag_yield); - // EXPECT_EQ(res, "ILC2Yield"); -} - - - -int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "rostest_ulc"); - ros::NodeHandle nh; - auto res = RUN_ALL_TESTS(); - return res; -} - diff --git a/unobstructed_lanechange/test/test_unobstructed_lanechange_plugin.cpp b/unobstructed_lanechange/test/test_unobstructed_lanechange_plugin.cpp deleted file mode 100644 index 3bd59ece28..0000000000 --- a/unobstructed_lanechange/test/test_unobstructed_lanechange_plugin.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - -#include "unobstructed_lanechange.h" -#include -#include -#include -#include -#include -#include "TestHelpers.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace unobstructed_lanechange -{ - TEST(UnobstructedLaneChangePlugin,Testusingosm){ - // File to process. - std::string path = ros::package::getPath("basic_autonomy"); - std::string file = "/resource/map/town01_vector_map_lane_change.osm"; - file = path.append(file); - lanelet::Id start_id = 101; - lanelet::Id lane_change_start_id = 101; - lanelet::Id end_id = 107; - int projector_type = 0; - std::string target_frame; - lanelet::ErrorMessages load_errors; - // Parse geo reference info from the original lanelet map (.osm) - lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame); - lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); - lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors); - if (map->laneletLayer.size() == 0) - { - FAIL() << "Input map does not contain any lanelets"; - } - std::shared_ptr cmw=std::make_shared(); - cmw->carma_wm::CARMAWorldModel::setMap(map); - //Set Route - carma_wm::test::setRouteByIds({start_id,end_id},cmw); - cmw->carma_wm::CARMAWorldModel::setMap(map); - //get starting position - auto llt=map.get()->laneletLayer.get(start_id); - lanelet::LineString3d left_bound=llt.leftBound(); - lanelet::LineString3d right_bound=llt.rightBound(); - geometry_msgs::PoseStamped left; - geometry_msgs::PoseStamped right; - for(lanelet::Point3d& p : left_bound) - { - left.pose.position.x=p.x(); - left.pose.position.y=p.y(); - left.pose.position.z=p.z(); - - } - for(lanelet::Point3d& p : right_bound) - { - right.pose.position.x=p.x(); - right.pose.position.y=p.y(); - right.pose.position.z=p.z(); - } - geometry_msgs::PoseStamped curr_pos; - curr_pos.pose.position.x=(left.pose.position.x+right.pose.position.x)/2; - curr_pos.pose.position.y=(left.pose.position.y+right.pose.position.y)/2; - curr_pos.pose.position.z=(left.pose.position.z+right.pose.position.z)/2; - - curr_pos.pose.orientation.x=0.0; - curr_pos.pose.orientation.y=0.0; - curr_pos.pose.orientation.z=0.0; - curr_pos.pose.orientation.w=0.0; - - //Define arguments for function maneuvers_to_points - UnobstructedLaneChangePlugin worker; - ros::Time::init(); //Initialize ros::Time - - //get ending downtrack from lanelet id - double ending_downtrack; - auto shortest_path = cmw->getRoute()->shortestPath(); - - lanelet::BasicPoint2d veh_pos=shortest_path[0].centerline2d().front(); - double starting_downtrack = cmw->routeTrackPos(veh_pos).downtrack; - ending_downtrack = cmw->routeTrackPos(shortest_path.back().centerline2d().back()).downtrack; - - worker.wm_ = cmw; - - //Define lane change maneuver - cav_msgs::Maneuver maneuver; - maneuver.type=cav_msgs::Maneuver::LANE_CHANGE; - maneuver.lane_change_maneuver.start_dist = starting_downtrack; - maneuver.lane_change_maneuver.end_dist = ending_downtrack; - maneuver.lane_change_maneuver.start_speed = 5.0; - maneuver.lane_change_maneuver.start_time = ros::Time::now(); - //calculate end_time assuming constant acceleration - double acc = pow(maneuver.lane_change_maneuver.start_speed,2)/(2*(ending_downtrack - starting_downtrack)); - double end_time = maneuver.lane_change_maneuver.start_speed/acc; - maneuver.lane_change_maneuver.end_speed = 25.0; - maneuver.lane_change_maneuver.end_time = ros::Time(end_time + 10.0); - maneuver.lane_change_maneuver.starting_lane_id = std::to_string(lane_change_start_id); - maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); - - std::vector maneuvers; - maneuvers.push_back(maneuver); - worker.current_speed_ = maneuver.lane_change_maneuver.start_speed; - cav_msgs::VehicleState vehicle_state; - vehicle_state.x_pos_global = veh_pos.x(); - vehicle_state.y_pos_global = veh_pos.y(); - - - /* Test PlanTrajectory cb */ - cav_srvs::PlanTrajectoryRequest req; - cav_srvs::PlanTrajectoryResponse resp; - - ros::Time::init(); - req.maneuver_plan.planning_start_time = ros::Time::now(); - req.maneuver_plan.planning_completion_time = req.maneuver_plan.planning_start_time + ros::Duration(10.0); - req.vehicle_state.x_pos_global = veh_pos.x(); - req.vehicle_state.y_pos_global = veh_pos.y(); - req.vehicle_state.longitudinal_vel = maneuver.lane_change_maneuver.start_speed; - - std::vector maneuvers_msg; - - // Create a second maneuver of a different type to test the final element in resp.related_maneuvers - cav_msgs::Maneuver maneuver2; - maneuver2.type = cav_msgs::Maneuver::LANE_FOLLOWING; - - maneuvers_msg.push_back(maneuver); - maneuvers_msg.push_back(maneuver2); - req.maneuver_plan.maneuvers = maneuvers_msg; - req.maneuver_index_to_plan = 0; - bool isTrajectory = worker.plan_trajectory_cb(req,resp); - - EXPECT_TRUE(isTrajectory); - EXPECT_TRUE(resp.trajectory_plan.trajectory_points.size() > 2); - EXPECT_EQ(0, resp.related_maneuvers.back()); - - } - - TEST(InLaneCruisingPluginTest, test_verify_yield) -{ - - UnobstructedLaneChangePlugin plugin; - - std::vector trajectory_points; - - ros::Time startTime(ros::Time::now()); - - cav_msgs::TrajectoryPlanPoint point_2; - point_2.x = 5.0; - point_2.y = 0.0; - point_2.target_time = startTime + ros::Duration(1); - point_2.lane_id = "1"; - trajectory_points.push_back(point_2); - - cav_msgs::TrajectoryPlanPoint point_3; - point_3.x = 10.0; - point_3.y = 0.0; - point_3.target_time = startTime + ros::Duration(2); - point_3.lane_id = "1"; - trajectory_points.push_back(point_3); - - - cav_msgs::TrajectoryPlan tp; - tp.trajectory_id = "yield"; - tp.trajectory_points = trajectory_points; - - std::string traj_id = "yield"; - bool res = plugin.validate_yield_plan(tp, traj_id); - ASSERT_TRUE(plugin.validate_yield_plan(tp, traj_id)); - std::string traj_id2 = "yieldd"; - ASSERT_FALSE(plugin.validate_yield_plan(tp, traj_id2)); - - cav_msgs::TrajectoryPlan tp2; - - cav_msgs::TrajectoryPlanPoint point_4; - point_4.x = 5.0; - point_4.y = 0.0; - point_4.target_time = startTime + ros::Duration(1); - point_4.lane_id = "1"; - tp2.trajectory_points.push_back(point_4); - - ASSERT_FALSE(plugin.validate_yield_plan(tp2, traj_id)); - - cav_msgs::TrajectoryPlan tp3; - - cav_msgs::TrajectoryPlanPoint point_5; - point_5.x = 5.0; - point_5.y = 0.0; - point_5.target_time = startTime; - point_5.lane_id = "1"; - tp3.trajectory_points.push_back(point_5); - - cav_msgs::TrajectoryPlanPoint point_6; - point_6.x = 10.0; - point_6.y = 0.0; - point_6.target_time = startTime + ros::Duration(1); - point_6.lane_id = "1"; - tp3.trajectory_points.push_back(point_6); - - ASSERT_FALSE(plugin.validate_yield_plan(tp2, traj_id)); - - -} - - -} - -// Run all the tests -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - auto res = RUN_ALL_TESTS(); - return res; -} \ No newline at end of file diff --git a/unobstructed_lanechange/test/ulc_plugin.test b/unobstructed_lanechange/test/ulc_plugin.test deleted file mode 100644 index 5b3290c6ce..0000000000 --- a/unobstructed_lanechange/test/ulc_plugin.test +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - \ No newline at end of file diff --git a/wz_strategic_plugin/CMakeLists.txt b/wz_strategic_plugin/CMakeLists.txt deleted file mode 100644 index a1226de9ab..0000000000 --- a/wz_strategic_plugin/CMakeLists.txt +++ /dev/null @@ -1,96 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(wz_strategic_plugin) - -find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) -carma_package() - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages - -set(PKG_CATKIN_DEPS - roscpp - cav_srvs - cav_msgs - carma_utils - carma_wm - lanelet2_core - lanelet2_traffic_rules -) - -find_package(catkin REQUIRED COMPONENTS - ${PKG_CATKIN_DEPS} -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED) - - -catkin_package( - CATKIN_DEPENDS ${PKG_CATKIN_DEPS} - DEPENDS Boost -) - -########### -## Build ## -########### - -include_directories( - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - include -) - -# Build Lib -add_library(${PROJECT_NAME} - src/wz_strategic_plugin.cpp - src/wz_states.cpp - src/wz_state_transition_table.cpp -) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -# Build Node Executable -add_executable( ${PROJECT_NAME}_node - src/main.cpp -) - -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -############# -## Install ## -############# - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Install C++ -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -catkin_add_gmock(${PROJECT_NAME}-test - test/test_main.cpp - test/test_fixture.h - test/test_strategic_plugin_helpers.cpp - test/test_transition_table.cpp - test/test_strategic_plugin.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) - -target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/wz_strategic_plugin/config/parameters.yaml b/wz_strategic_plugin/config/parameters.yaml deleted file mode 100644 index a8730f97a7..0000000000 --- a/wz_strategic_plugin/config/parameters.yaml +++ /dev/null @@ -1,26 +0,0 @@ -# Double: multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities -vehicle_decel_limit_multiplier : 0.5 - -# Double: The minimum distance in meters that the vehicle can be at before requiring a transition to the APPROACH state -min_approach_distance : 30.0 - -# Double: A buffer infront of the stopping location which will still be considered a valid stop. Units in meters -stopping_location_buffer : 5.0 - -# Double: A buffer in seconds around the green phase which will reduce the phase length such that vehicle still considers it non-green -green_light_time_buffer : 2.0 - -# Double: The minimum period in seconds which a maneuver plan must cover if the plugin wishes to control the whole plan -min_maneuver_planning_period : 15.1 - -# String: The name to use for this plugin during comminications with the arbitrator -strategic_plugin_name : WorkZonePlugin - -# String: The name of the tactical plugin to use for Lane Following trajectory planning -lane_following_plugin_name : InLaneCruisingPlugin - -# String: The name of the plugin to use for stop and wait trajectory planning -stop_and_wait_plugin_name : StopAndWaitPlugin - -# String: The name of the plugin to use for intersection transit trajectory planning -intersection_transit_plugin_name : IntersectionTransitPlugin \ No newline at end of file diff --git a/wz_strategic_plugin/include/wz_strategic_plugin/wz_state_transition_table.h b/wz_strategic_plugin/include/wz_strategic_plugin/wz_state_transition_table.h deleted file mode 100644 index 3e987cfc04..0000000000 --- a/wz_strategic_plugin/include/wz_strategic_plugin/wz_state_transition_table.h +++ /dev/null @@ -1,83 +0,0 @@ -#pragma once -/* - * Copyright (C) 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 "wz_states.h" -#include - -namespace wz_strategic_plugin -{ -/** - * \brief Class defining the state transition table behavior for the WorkZone Strategic Plugin - */ -class WorkZoneStateTransitionTable -{ -public: - using TransitionCallback = std::function; - - /** - * \brief Default Constructor - */ - WorkZoneStateTransitionTable() = default; - - /** - * \brief Returns the current state - * \return Current state - */ - TransitState getState() const; - - /** - * \brief Trigger signal for the transition table. - * - * \param signal The signal for the transition table to evaluate - */ - void signal(TransitEvent signal); - - /** - * \brief Callback setting function. The provided callback will be triggered any time the current state changes to a - * new state. - * - * \param cb The callback function which will be provided with the previous state, new current state, and the signal - * which caused the transition. - */ - void setTransitionCallback(TransitionCallback cb); - -private: - //! Current state. This state should only ever be set using the setAndLogState() function. - TransitState state_ = TransitState::UNAVAILABLE; - - TransitionCallback transition_callback_; - - // Helper functions for processing each provided signal based on the current state - void signalWhenUNAVAILABLE(TransitEvent signal); - void signalWhenAPPROACHING(TransitEvent signal); - void signalWhenWAITING(TransitEvent signal); - void signalWhenDEPARTING(TransitEvent signal); - - /** - * \brief Helper function for logging the provide signal - * \param signal The signal to be logged - */ - void logDebugSignal(TransitEvent signal) const; - - /** - * \brief Function to change the current state and log the details of the transition. - * - * \param new_state The state to set. - * \param source_signal The signal which caused the new_state to be set - */ - void setAndLogState(TransitState new_state, TransitEvent source_signal); -}; -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/wz_strategic_plugin/include/wz_strategic_plugin/wz_states.h b/wz_strategic_plugin/include/wz_strategic_plugin/wz_states.h deleted file mode 100644 index d742aa9625..0000000000 --- a/wz_strategic_plugin/include/wz_strategic_plugin/wz_states.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once -/* - * Copyright (C) 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 - -namespace wz_strategic_plugin -{ - -//! @brief Enum describing the possible states of the WorkZone Strategic Plugin -enum class TransitState -{ - UNAVAILABLE, // State representing that there are no applicable intersections in range, so the plugin cannot plan - APPROACHING, // State representing that the vehicle is approaching an intersection - WAITING, // State representing that the vehicle is stopped and waiting at a light - DEPARTING // State representing that the vehicle is traversing the intersection -}; - -/** - * \brief Stream operator for TransitStates enum. - */ -std::ostream& operator<<(std::ostream& os, TransitState s); - -//! @brief Enum describing the possible signals to change the current TransitState -enum class TransitEvent -{ - IN_STOPPING_RANGE, // Transition event representing that the vehicle is in the stopping range of a light - STOPPED, // Transition event representing that the vehicle has come to a full stop - CROSSED_STOP_BAR, // Transition event representing that the vehicle has crossed the stop bar - RED_TO_GREEN_LIGHT, // Transition event representing that the current light of interest has changed from red to green - INTERSECTION_EXIT // Transition event representing that the end of the current intersection was crossed -}; -/** - * \brief Stream operator for TransitEvent enum. - */ -std::ostream& operator<<(std::ostream& os, TransitEvent s); - -} // namespace localizer \ No newline at end of file diff --git a/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h b/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h deleted file mode 100644 index 4f8f48bc59..0000000000 --- a/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin.h +++ /dev/null @@ -1,291 +0,0 @@ -#pragma once - -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include "wz_strategic_plugin/wz_state_transition_table.h" -#include "wz_strategic_plugin/wz_strategic_plugin_config.h" -#include "wz_strategic_plugin/wz_states.h" - -namespace wz_strategic_plugin -{ -class WzStrategicPlugin -{ -public: - /** - * \brief Constructor - * - * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data - * \param config The configuration to be used for this object - */ - WzStrategicPlugin(carma_wm::WorldModelConstPtr wm, WzStrategicPluginConfig config); - - /** - * \brief Service callback for arbitrator maneuver planning - * \param req Plan maneuver request - * \param[out] resp Plan maneuver response with a list of maneuver plan - * \return If service call successed - */ - bool planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp); - - /** - * \brief Returns the current plugin discovery message reflecting system status - * - * \return cav_msgs::Plugin The plugin discovery message - */ - cav_msgs::Plugin getDiscoveryMsg() const; - -private: - /** - * \brief Struct representing a vehicle state for the purposes of planning - */ - struct VehicleState - { - ros::Time stamp; // Timestamp of this state data - double downtrack; // The downtrack of the vehicle along the route at time stamp - double speed; // The speed of the vehicle at time stamp - lanelet::Id lane_id; // The current lane id of the vehicle at time stamp - }; - - /** - * \brief Method for performing maneuver planning when the current plugin state is TransitState::UNAVAILABLE - * Therefor no maneuvers are planned but the system checks for the precense of traffic lights ahead of it - * - * \param req Plan maneuver request - * \param[out] resp Plan maneuver response with a list of maneuver plan - * \param current_state The current state of the vehicle at the start of planning - * \param traffic_lights A list of traffic lights along the vehicle's route which lie in front of the vehicle - * - */ - void planWhenUNAVAILABLE(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, - const VehicleState& current_state, - const std::vector& traffic_lights); - - /** - * \brief Method for performing maneuver planning when the current plugin state is TransitState::APPROACHING - * Therefor the planned maneuvers deal with approaching a traffic light - * - * \param req Plan maneuver request - * \param[out] resp Plan maneuver response with a list of maneuver plan - * \param current_state The current state of the vehicle at the start of planning - * \param traffic_lights A list of traffic lights along the vehicle's route which lie in front of the vehicle - * - */ - void planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, - const VehicleState& current_state, - const std::vector& traffic_lights); - - /** - * \brief Method for performing maneuver planning when the current plugin state is TransitState::WAITING - * Therefor the planned maneuvers deal with waiting at a red light - * - * \param req Plan maneuver request - * \param[out] resp Plan maneuver response with a list of maneuver plan - * \param current_state The current state of the vehicle at the start of planning - * \param traffic_lights A list of traffic lights along the vehicle's route which lie in front of the vehicle - * - */ - void planWhenWAITING(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, - const VehicleState& current_state, - const std::vector& traffic_lights); - - /** - * \brief Method for performing maneuver planning when the current plugin state is TransitState::DEPARTING - * Therefor the planned maneuvers deal with the transit of the intersection once the stop bar has been crossed - * - * \param req Plan maneuver request - * \param[out] resp Plan maneuver response with a list of maneuver plan - * \param current_state The current state of the vehicle at the start of planning - * \param intersection_end_downtrack The ending downtrack in meters of the current intersection - * \param intersection_speed_limit The speed limit of the current intersection - * - */ - void planWhenDEPARTING(const cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp, - const VehicleState& current_state, double intersection_end_downtrack, - double intersection_speed_limit); - - /** - * \brief Compose a lane keeping maneuver message based on input params - * - * \param start_dist Start downtrack distance of the current maneuver - * \param end_dist End downtrack distance of the current maneuver - * \param start_speed Start speed of the current maneuver - * \param target_speed Target speed pf the current maneuver, usually it is the lanelet speed limit - * \param start_time The starting time of the maneuver - * \param end_time The ending time of the maneuver - * \param lane_ids List of lanelet IDs that the current maneuver traverses. Message expects these to be contiguous and - * end to end - * - * \return A lane keeping maneuver message which is ready to be published - */ - cav_msgs::Maneuver composeLaneFollowingManeuverMessage(double start_dist, double end_dist, double start_speed, - double target_speed, ros::Time start_time, ros::Time end_time, - std::vector lane_ids) const; - - /** - * \brief Compose a stop and wait maneuver message based on input params - * - * \param current_dist Start downtrack distance of the current maneuver - * \param end_dist End downtrack distance of the current maneuver - * \param start_speed Start speed of the current maneuver - * \param starting_lane_id The starting lanelet id of this maneuver - * \param ending_lane_id The ending lanelet id of this maneuver - * \param start_time The starting time of the maneuver - * \param end_time The ending time of the maneuver - * \param stopping_accel Acceleration used for calculating the stopping distance - * - * \return A stop and wait maneuver message which is ready to be published - */ - cav_msgs::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed, - const lanelet::Id& starting_lane_id, - const lanelet::Id& ending_lane_id, ros::Time start_time, - ros::Time end_time, double stopping_accel) const; - - /** - * \brief Compose a intersection transit maneuver message based on input params - * - * \param start_dist Start downtrack distance of the current maneuver - * \param end_dist End downtrack distance of the current maneuver - * \param start_speed Start speed of the current maneuver - * \param target_speed Target speed pf the current maneuver, usually it is the lanelet speed limit - * \param start_time The starting time of the maneuver - * \param end_time The ending time of the maneuver - * \param starting_lane_id The starting lanelet id of this maneuver - * \param ending_lane_id The ending lanelet id of this maneuver - * - * \return A intersection transit maneuver maneuver message which is ready to be published - */ - cav_msgs::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed, - double target_speed, ros::Time start_time, ros::Time end_time, - const lanelet::Id& starting_lane_id, - const lanelet::Id& ending_lane_id) const; - - /** - * \brief Helper method to evaluate if the given traffic light state is supported by this plugin - * - * \param state The state to evaluate - * - * \return true if the state is supported, flase otherwise - */ - bool supportedLightState(lanelet::CarmaTrafficSignalState state) const; - - /** - * \brief Helper method that checks both if the input optional light state is set and if the state it contains is - * supported via supportedLightState. - * - * \param optional_state An optional light state and its min_end_time pair. If this is unset the method will return false - * \param source_time The time used to optain the optional light state. This is used for logging only - * - * \return True if the optional is set and the contained state signal is supported. False otherwise - */ - bool validLightState(const boost::optional>& optional_state, - const ros::Time& source_time) const; - - /** - * \brief Helper method to extract the initial vehicle state from the planning request method based on if the - * prior_plan was set or not. - * - * \param req The maneuver planning request to extract the vehicle state from - * - * \return The extracted VehicleState - */ - VehicleState extractInitialState(const cav_srvs::PlanManeuversRequest& req) const; - - /** - * \brief Helper method which calls carma_wm::WorldModel::getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, - * bounds_inclusive) and throws and exception if the returned list of lanelets is empty. See the referenced method for additional - * details on parameters. - */ - std::vector getLaneletsBetweenWithException(double start_downtrack, double end_downtrack, - bool shortest_path_only = false, - bool bounds_inclusive = true) const; - - /** - * \brief Helper method to use basic kinematics to compute an estimated stopping distance from from the inputs - * - * \param v The initial velocity in m/s - * \param a The deceleration in m/s^2 - * - * \return The estimated stopping distance in meters - */ - double estimate_distance_to_stop(double v, double a) const; - - /** - * \brief Helper method to use basic kinematics to compute an estimated stopping time from from the inputs - * - * \param d The distance to travel in meters - * \param v The initial velocity in m/s - * - * \return The estimated stopping time in seconds - */ - double estimate_time_to_stop(double d, double v) const; - - /** - * \brief Given a Lanelet, find it's associated Speed Limit - * - * \param llt Constant Lanelet object - * - * \throw std::invalid_argument if the speed limit could not be retrieved - * - * \return value of speed limit in mps - */ - double findSpeedLimit(const lanelet::ConstLanelet& llt) const; - - ////////// VARIABLES /////////// - - //! World Model pointer - carma_wm::WorldModelConstPtr wm_; - - //! Config containing configurable algorithm parameters - WzStrategicPluginConfig config_; - - //! State Machine Transition table - WorkZoneStateTransitionTable transition_table_; - - //! Plugin discovery message - cav_msgs::Plugin plugin_discovery_msg_; - - //! Cache variables for storing the current intersection state between state machine transitions - boost::optional intersection_speed_; - boost::optional intersection_end_downtrack_; - - //Unit Tests - FRIEND_TEST(WorkZoneTestFixture, getDiscoveryMsg); - FRIEND_TEST(WorkZoneTestFixture, supportedLightState); - FRIEND_TEST(WorkZoneTestFixture, estimate_distance_to_stop); - FRIEND_TEST(WorkZoneTestFixture, estimate_time_to_stop); - FRIEND_TEST(WorkZoneTestFixture, extractInitialState); - FRIEND_TEST(WorkZoneTestFixture, validLightState); - FRIEND_TEST(WorkZoneTestFixture, getLaneletsBetweenWithException); - FRIEND_TEST(WorkZoneTestFixture, composeLaneFollowingManeuverMessage); - FRIEND_TEST(WorkZoneTestFixture, composeStopAndWaitManeuverMessage); - FRIEND_TEST(WorkZoneTestFixture, composeIntersectionTransitMessage); - FRIEND_TEST(WorkZoneTestFixture, findSpeedLimit); - -}; -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin_config.h b/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin_config.h deleted file mode 100644 index 32d6759676..0000000000 --- a/wz_strategic_plugin/include/wz_strategic_plugin/wz_strategic_plugin_config.h +++ /dev/null @@ -1,58 +0,0 @@ -#pragma once -/* - * Copyright (C) 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 - -namespace wz_strategic_plugin -{ -//! @brief Struct to store the configuration settings for the WzStrategicPlugin class -struct WzStrategicPluginConfig -{ - //! The maximum allowable vehicle deceleration limit in m/s - double vehicle_decel_limit = 2.0; - - //! A multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities - double vehicle_decel_limit_multiplier = 0.75; - - //! The length parameter of the participant vehicle used to help calculate the distance before stopping at a red traffic signal - double vehicle_length = 0.0; - - //! The minimum distance in meters that the vehicle can be at before requiring a transition to the APPROACH state - double min_approach_distance = 30.0; - - //! A buffer infront of the stopping location which will still be considered a valid stop. Units in meters - double stopping_location_buffer = 3.0; - - //! A buffer in seconds around the green phase which will reduce the phase length such that vehicle still considers it non-green - double green_light_time_buffer = 2.0; - - //! The minimum period in seconds which a maneuver plan must cover if the plugin wishes to control the whole plan - double min_maneuver_planning_period = 15.1; - - //! The name to use for this plugin during comminications with the arbitrator - std::string strategic_plugin_name = "WorkZonePlugin"; - - //! The name of the tactical plugin to use for Lane Following trajectory planning - std::string lane_following_plugin_name = "InLaneCruisingPlugin"; - - //! The name of the plugin to use for stop and wait trajectory planning - std::string stop_and_wait_plugin_name = "StopAndWaitPlugin"; - - //! The name of the plugin to use for intersection transit trajectory planning - std::string intersection_transit_plugin_name = "IntersectionTransitPlugin"; -}; -} // namespace localizer \ No newline at end of file diff --git a/wz_strategic_plugin/launch/wz_strategic_plugin.launch b/wz_strategic_plugin/launch/wz_strategic_plugin.launch deleted file mode 100644 index 1539d6b653..0000000000 --- a/wz_strategic_plugin/launch/wz_strategic_plugin.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - diff --git a/wz_strategic_plugin/package.xml b/wz_strategic_plugin/package.xml deleted file mode 100644 index d98bb0aabb..0000000000 --- a/wz_strategic_plugin/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - wz_strategic_plugin - 1.0.0 - The strategic plugin generates maneuvers to make the vehicle go through the workzone. - carma - Apache License 2.0 - carma - catkin - roscpp - cav_srvs - cav_msgs - carma_utils - carma_wm - lanelet2_core - lanelet2_traffic_rules - carma_cmake_common - \ No newline at end of file diff --git a/wz_strategic_plugin/src/main.cpp b/wz_strategic_plugin/src/main.cpp deleted file mode 100644 index 3d88123881..0000000000 --- a/wz_strategic_plugin/src/main.cpp +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright (C) 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 "wz_strategic_plugin/wz_strategic_plugin.h" -#include "wz_strategic_plugin/wz_strategic_plugin_config.h" - -int main(int argc, char** argv) -{ - // Initialize ros connection - ros::init(argc, argv, "wz_strategic_plugin"); - - // Setup node handles - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - // Create publishers - ros::Publisher plugin_discovery_pub = nh.advertise("plugin_discovery", 1); - - // Initialize world model - carma_wm::WMListener wml; - - // Load Parameters - wz_strategic_plugin::WzStrategicPluginConfig config; - - // clang-format off - pnh.param("/vehicle_acceleration_limit", config.vehicle_decel_limit, config.vehicle_decel_limit); - pnh.param("vehicle_decel_limit_multiplier", config.vehicle_decel_limit_multiplier, config.vehicle_decel_limit_multiplier); - pnh.param("min_approach_distance", config.min_approach_distance, config.min_approach_distance); - pnh.param("stopping_location_buffer", config.stopping_location_buffer, config.stopping_location_buffer); - pnh.param("green_light_time_buffer", config.green_light_time_buffer, config.green_light_time_buffer); - pnh.param("min_maneuver_planning_period", config.min_maneuver_planning_period, config.min_maneuver_planning_period); - pnh.param("strategic_plugin_name", config.strategic_plugin_name, config.strategic_plugin_name); - pnh.param("lane_following_plugin_name", config.lane_following_plugin_name, config.lane_following_plugin_name); - pnh.param("stop_and_wait_plugin_name", config.stop_and_wait_plugin_name, config.stop_and_wait_plugin_name); - pnh.param("intersection_transit_plugin_name", config.intersection_transit_plugin_name, config.intersection_transit_plugin_name); - // pnh.param("/vehicle_length", config.vehicle_length, config.vehicle_length); - // clang-format on - - // Construct plugin - wz_strategic_plugin::WzStrategicPlugin wzp(wml.getWorldModel(), config); - - // Setup callback connections - ros::ServiceServer plan_maneuver_srv = - nh.advertiseService("plugins/" + config.strategic_plugin_name + "/plan_maneuvers", &wz_strategic_plugin::WzStrategicPlugin::planManeuverCb, &wzp); - - ros::Timer discovery_pub_timer = - nh.createTimer(ros::Duration(ros::Rate(10.0)), [&wzp, &plugin_discovery_pub](const auto&) { - plugin_discovery_pub.publish(wzp.getDiscoveryMsg()); - }); - - // Start - ros::CARMANodeHandle::spin(); - - return 0; -}; \ No newline at end of file diff --git a/wz_strategic_plugin/src/wz_state_transition_table.cpp b/wz_strategic_plugin/src/wz_state_transition_table.cpp deleted file mode 100644 index faa00cff4e..0000000000 --- a/wz_strategic_plugin/src/wz_state_transition_table.cpp +++ /dev/null @@ -1,143 +0,0 @@ - -/* - * Copyright (C) 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 "wz_strategic_plugin/wz_state_transition_table.h" - -namespace wz_strategic_plugin -{ - // TODO clean up -// THERE SHOULD BE FOUR OPERATIONAL MODES FOR THIS PLUGIN -// UNAVAILABLE { On In Intersection Range Event -> APPROACH } -// APPROACHING { On Stopped Event -> WAITING, On Crossed Stop Bar -> DEPARTURE} -// WAITING { On Green Light Event -> DEPARTURE} -// DEPARTING { On exit intersection -> UNAVAILABLE} // TODO remove - -TransitState WorkZoneStateTransitionTable::getState() const -{ - return state_; -} - -void WorkZoneStateTransitionTable::signal(TransitEvent signal) -{ - switch (state_) - { - case TransitState::UNAVAILABLE: - signalWhenUNAVAILABLE(signal); - break; - - case TransitState::APPROACHING: - signalWhenAPPROACHING(signal); - break; - - case TransitState::WAITING: - signalWhenWAITING(signal); - break; - - case TransitState::DEPARTING: - signalWhenDEPARTING(signal); - break; - - default: - throw std::invalid_argument("Transition table in unsupported state"); - } -} - -void WorkZoneStateTransitionTable::setTransitionCallback(TransitionCallback cb) -{ - transition_callback_ = cb; -} - -void WorkZoneStateTransitionTable::signalWhenUNAVAILABLE(TransitEvent signal) -{ - if (signal == TransitEvent::IN_STOPPING_RANGE) - { - setAndLogState(TransitState::APPROACHING, signal); - } - else - { - logDebugSignal(signal); - } -} - -void WorkZoneStateTransitionTable::signalWhenAPPROACHING(TransitEvent signal) -{ - switch (signal) - { - case TransitEvent::STOPPED: - setAndLogState(TransitState::WAITING, signal); - break; - - case TransitEvent::CROSSED_STOP_BAR: - setAndLogState(TransitState::DEPARTING, signal); - break; - - default: - logDebugSignal(signal); - break; - } -} - -void WorkZoneStateTransitionTable::signalWhenWAITING(TransitEvent signal) -{ - if (signal == TransitEvent::RED_TO_GREEN_LIGHT) - { - setAndLogState(TransitState::DEPARTING, signal); - } - else - { - logDebugSignal(signal); - } -} - -void WorkZoneStateTransitionTable::signalWhenDEPARTING(TransitEvent signal) -{ - if (signal == TransitEvent::INTERSECTION_EXIT) - { - setAndLogState(TransitState::UNAVAILABLE, signal); - } - else - { - logDebugSignal(signal); - } -} - -void WorkZoneStateTransitionTable::logDebugSignal(TransitEvent signal) const -{ - ROS_DEBUG_STREAM("WorkZoneStateTransitionTable received unsupported signal of " << signal << " while in state " - << state_); -} - -void WorkZoneStateTransitionTable::setAndLogState(TransitState new_state, TransitEvent source_signal) -{ - if (new_state == state_) - { - return; // State was unchanged no need to log or trigger callbacks - } - - ROS_INFO_STREAM("WorkZoneStateTransitionTable changed WorkZone Strategic Plugin state from " - << state_ << " to " << new_state << " because of signal " << source_signal); - - TransitState prev_state = state_; - state_ = new_state; // Set new state - - if (transition_callback_) // Trigger callback if available - { - transition_callback_(prev_state, state_, source_signal); - } -} - -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/wz_strategic_plugin/src/wz_states.cpp b/wz_strategic_plugin/src/wz_states.cpp deleted file mode 100644 index 0c9062a017..0000000000 --- a/wz_strategic_plugin/src/wz_states.cpp +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright (C) 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 "wz_strategic_plugin/wz_states.h" - -namespace wz_strategic_plugin -{ -std::ostream& operator<<(std::ostream& os, TransitState s) -{ - os << "TransitState::"; - switch (s) - { // clang-format off - case TransitState::UNAVAILABLE : os << "UNAVAILABLE"; break; - case TransitState::APPROACHING: os << "APPROACHING"; break; - case TransitState::WAITING : os << "WAITING"; break; - case TransitState::DEPARTING : os << "DEPARTING"; break; - default: os.setstate(std::ios_base::failbit); - } // clang-format on - return os; -} - -std::ostream& operator<<(std::ostream& os, TransitEvent s) -{ - os << "TransitEvent::"; - switch (s) - { // clang-format off - case TransitEvent::IN_STOPPING_RANGE : os << "IN_STOPPING_RANGE"; break; - case TransitEvent::STOPPED: os << "STOPPED"; break; - case TransitEvent::CROSSED_STOP_BAR : os << "CROSSED_STOP_BAR"; break; - case TransitEvent::RED_TO_GREEN_LIGHT : os << "RED_TO_GREEN_LIGHT"; break; - case TransitEvent::INTERSECTION_EXIT : os << "INTERSECTION_EXIT"; break; - default: os.setstate(std::ios_base::failbit); - } // clang-format on - return os; -} - -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/wz_strategic_plugin/src/wz_strategic_plugin.cpp b/wz_strategic_plugin/src/wz_strategic_plugin.cpp deleted file mode 100644 index c56081ff15..0000000000 --- a/wz_strategic_plugin/src/wz_strategic_plugin.cpp +++ /dev/null @@ -1,566 +0,0 @@ -/* - * Copyright (C) 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 "wz_strategic_plugin/wz_strategic_plugin.h" -#include "wz_strategic_plugin/wz_states.h" - -#define GET_MANEUVER_PROPERTY(mvr, property) \ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? \ - (mvr).intersection_transit_left_turn_maneuver.property : \ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? \ - (mvr).intersection_transit_right_turn_maneuver.property : \ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? \ - (mvr).intersection_transit_straight_maneuver.property : \ - ((mvr).type == cav_msgs::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property : \ - (mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property : \ - throw new std::invalid_argument("GET_MANEUVER_" \ - "PROPERTY " \ - "(property) " \ - "called on " \ - "maneuver with " \ - "invalid type " \ - "id")))))) - -namespace wz_strategic_plugin -{ -WzStrategicPlugin::WzStrategicPlugin(carma_wm::WorldModelConstPtr wm, WzStrategicPluginConfig config) - : wm_(wm), config_(config) -{ - plugin_discovery_msg_.name = config_.strategic_plugin_name; - plugin_discovery_msg_.version_id = "v1.0"; - plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = true; - plugin_discovery_msg_.type = cav_msgs::Plugin::STRATEGIC; - plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers"; -}; - -cav_msgs::Plugin WzStrategicPlugin::getDiscoveryMsg() const -{ - return plugin_discovery_msg_; -} - -bool WzStrategicPlugin::supportedLightState(lanelet::CarmaTrafficSignalState state) const -{ - switch (state) - { - // NOTE: Following cases are intentional fall through. - // Supported light states - case lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN: // Solid Red - case lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE: // Yellow Solid no chance of conflicting traffic - case lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED: // Solid Green no chance of conflict traffic - // (normally used with arrows) - return true; - - // Unsupported light states - case lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED: // Solid Green there could be conflict traffic - case lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE: // Yellow Solid there is a chance of conflicting - // traffic - case lanelet::CarmaTrafficSignalState::UNAVAILABLE: // No data available - case lanelet::CarmaTrafficSignalState::DARK: // Light is non-functional - case lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED: // Flashing Red - - case lanelet::CarmaTrafficSignalState::PRE_MOVEMENT: // Yellow Red transition (Found only in the EU) - // (normally used with arrows) - case lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC: // Yellow Flashing - default: - return false; - } -} - -double WzStrategicPlugin::estimate_distance_to_stop(double v, double a) const -{ - return (v * v) / (2.0 * a); -} - -double WzStrategicPlugin::estimate_time_to_stop(double d, double v) const // TODO remove -{ - return 2.0 * d / v; -}; - -WzStrategicPlugin::VehicleState WzStrategicPlugin::extractInitialState(const cav_srvs::PlanManeuversRequest& req) const -{ - VehicleState state; - if (!req.prior_plan.maneuvers.empty()) - { - ROS_DEBUG_STREAM("Provided with initial plan..."); - state.stamp = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_time); - state.downtrack = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_dist); - state.speed = GET_MANEUVER_PROPERTY(req.prior_plan.maneuvers.back(), end_speed); - state.lane_id = getLaneletsBetweenWithException(state.downtrack, state.downtrack, true).front().id(); - } - else - { - ROS_DEBUG_STREAM("No initial plan provided..."); - - state.stamp = req.header.stamp; - state.downtrack = req.veh_downtrack; - state.speed = req.veh_logitudinal_velocity; - state.lane_id = stoi(req.veh_lane_id); - } - ROS_DEBUG_STREAM("extractInitialState >>>> state.stamp: " << state.stamp); - ROS_DEBUG_STREAM("extractInitialState >>>> state.downtrack : " << state.downtrack ); - ROS_DEBUG_STREAM("extractInitialState >>>> state.speed: " << state.speed); - ROS_DEBUG_STREAM("extractInitialState >>>> state.lane_id: " << state.lane_id); - - return state; -} - -bool WzStrategicPlugin::validLightState(const boost::optional>& optional_state, - const ros::Time& source_time) const -{ - if (!optional_state) - { - ROS_WARN_STREAM("Traffic light data not available for source_time " << std::to_string(source_time.toSec())); - return false; - } - - lanelet::CarmaTrafficSignalState light_state = optional_state.get().second; - - if (!supportedLightState(light_state)) - { - ROS_ERROR_STREAM("WorkZone Plugin asked to handle CarmaTrafficSignalState: " << light_state - << " which is not supported."); - return false; - } - - return true; -} - -std::vector WzStrategicPlugin::getLaneletsBetweenWithException(double start_downtrack, - double end_downtrack, - bool shortest_path_only, - bool bounds_inclusive) const -{ - std::vector crossed_lanelets = - wm_->getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, bounds_inclusive); - - if (crossed_lanelets.size() == 0) - { - throw std::invalid_argument("getLaneletsBetweenWithException called but inputs do not cross any lanelets going " - "from: " + - std::to_string(start_downtrack) + " to: " + std::to_string(end_downtrack)); - } - - return crossed_lanelets; -} - -void WzStrategicPlugin::planWhenUNAVAILABLE(const cav_srvs::PlanManeuversRequest& req, - cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights) -{ - // Reset intersection state since in this state we are not yet known to be in or approaching an intersection - intersection_speed_ = boost::none; - intersection_end_downtrack_ = boost::none; - - if (traffic_lights.empty()) - { - ROS_DEBUG("No lights found along route. Returning maneuver plan unchanged"); - return; - } - - double traffic_light_down_track = - wm_->routeTrackPos(traffic_lights.front()->stopLine().front().front().basicPoint2d()).downtrack; - - ROS_DEBUG("traffic_light_down_track %f", traffic_light_down_track); - - double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack - config_.vehicle_length; - - ROS_DEBUG("distance_remaining_to_traffic_light %f", distance_remaining_to_traffic_light); - - double stopping_dist = estimate_distance_to_stop(current_state.speed, config_.vehicle_decel_limit_multiplier * - config_.vehicle_decel_limit); - - ROS_DEBUG_STREAM("Stopping distance: " << stopping_dist); - - double plugin_activation_distance = std::max(stopping_dist, config_.min_approach_distance); - - ROS_DEBUG_STREAM("plugin_activation_distance: " << plugin_activation_distance); - - if (distance_remaining_to_traffic_light <= plugin_activation_distance) - { - ROS_INFO_STREAM("Within intersection range"); - transition_table_.signal(TransitEvent::IN_STOPPING_RANGE); // Evaluate Signal - } - else - { - ROS_DEBUG_STREAM("Not within intersection range"); - } -} -// TODO should we handle when the vehicle is not going to make the light but doesn't have space to stop? -void WzStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversRequest& req, - cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights) -{ - if (traffic_lights.empty()) // If we are in the approaching state and there is no longer any lights ahead of us then - // the vehicle must have crossed the stop bar - { - transition_table_.signal(TransitEvent::CROSSED_STOP_BAR); - return; - } - - auto nearest_traffic_light = traffic_lights.front(); - - double traffic_light_down_track = - wm_->routeTrackPos(nearest_traffic_light->stopLine().front().front().basicPoint2d()).downtrack; - - ROS_DEBUG("traffic_light_down_track %f", traffic_light_down_track); - - double distance_remaining_to_traffic_light = traffic_light_down_track - current_state.downtrack; - - // If the vehicle is at a stop trigger the - constexpr double HALF_MPH_IN_MPS = 0.22352; - if (current_state.speed < HALF_MPH_IN_MPS && - fabs(distance_remaining_to_traffic_light) < config_.stopping_location_buffer) - { - transition_table_.signal(TransitEvent::STOPPED); // The vehicle has come to a stop at the light - return; - } - - // At this point we know the vehicle is within the activation distance and we know the current and next light phases - // All we need to now determine is if we should stop or if we should continue - - intersection_speed_ = findSpeedLimit(nearest_traffic_light->getControlStartLanelets().front()); - - ROS_DEBUG_STREAM("intersection_speed_: " << intersection_speed_.get()); - - intersection_end_downtrack_ = - wm_->routeTrackPos(nearest_traffic_light->getControlEndLanelets().back().centerline2d().back()).downtrack; - - ROS_DEBUG_STREAM("intersection_end_downtrack_: " << intersection_end_downtrack_.get()); - - // Estimate the time to reach the traffic light assuming we decelerate to the speed of the intersection before arrival - // In the case of higher accel limits this calculation should always overestimate the arrival time which should be - // fine as that would hit the following red phase. - double time_remaining_to_traffic_light = - (2.0 * (distance_remaining_to_traffic_light - config_.vehicle_length)) / - (intersection_speed_.get() + current_state.speed); // Kinematic Equation: 2*d / (vf + vi) = t - - ROS_DEBUG_STREAM("time_remaining_to_traffic_light: " << time_remaining_to_traffic_light); - - ros::Time light_arrival_time_at_freeflow = current_state.stamp + ros::Duration(time_remaining_to_traffic_light); - - ros::Time early_arrival_time_at_freeflow = - light_arrival_time_at_freeflow - ros::Duration(config_.green_light_time_buffer); - - ros::Time late_arrival_time_at_freeflow = - light_arrival_time_at_freeflow + ros::Duration(config_.green_light_time_buffer); - - ROS_DEBUG_STREAM("light_arrival_time_at_freeflow: " << std::to_string(light_arrival_time_at_freeflow.toSec())); - ROS_DEBUG_STREAM("early_arrival_time_at_freeflow: " << std::to_string(early_arrival_time_at_freeflow.toSec())); - ROS_DEBUG_STREAM("late_arrival_time_at_freeflow: " << std::to_string(late_arrival_time_at_freeflow.toSec())); - - auto early_arrival_state_at_freeflow_optional = nearest_traffic_light->predictState(lanelet::time::timeFromSec(early_arrival_time_at_freeflow.toSec())); - - if (!validLightState(early_arrival_state_at_freeflow_optional, early_arrival_time_at_freeflow)) - return; - - ROS_DEBUG_STREAM("early_arrival_state_at_freeflow: " << early_arrival_state_at_freeflow_optional.get().second); - - auto late_arrival_state_at_freeflow_optional = nearest_traffic_light->predictState(lanelet::time::timeFromSec(late_arrival_time_at_freeflow.toSec())); - - if (!validLightState(late_arrival_state_at_freeflow_optional, late_arrival_time_at_freeflow)) - return; - - ROS_DEBUG_STREAM("late_arrival_state_at_freeflow: " << late_arrival_state_at_freeflow_optional.get().second); - - // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver - std::vector crossed_lanelets = - getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true); - - // We will cross the light on the green phase even if we arrive early or late - if (early_arrival_state_at_freeflow_optional.get().second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED && - late_arrival_state_at_freeflow_optional.get().second == - lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) // Green light - { - - ROS_DEBUG_STREAM("Planning lane follow and intersection transit maneuvers"); - // TODO plan lane follow from current speed to intersection speed (does inlane-cruising even support this???) - - // TODO do we need to check for anything before pushing onto the plan? - resp.new_plan.maneuvers.push_back(composeLaneFollowingManeuverMessage( - current_state.downtrack, traffic_light_down_track - config_.vehicle_length, current_state.speed, intersection_speed_.get(), - current_state.stamp, light_arrival_time_at_freeflow, - lanelet::utils::transform(crossed_lanelets, [](const auto& ll) { return ll.id(); }))); - - double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track; - - ros::Time intersection_exit_time = - light_arrival_time_at_freeflow + ros::Duration(intersection_length / intersection_speed_.get()); - - resp.new_plan.maneuvers.push_back(composeIntersectionTransitMessage( - traffic_light_down_track - config_.vehicle_length, intersection_end_downtrack_.get(), intersection_speed_.get(), - intersection_speed_.get(), light_arrival_time_at_freeflow, intersection_exit_time, crossed_lanelets.back().id(), - nearest_traffic_light->getControlEndLanelets().back().id())); - } - else // Red or Yellow light - { - double stopping_accel = config_.vehicle_decel_limit_multiplier * config_.vehicle_decel_limit; - - ROS_DEBUG_STREAM("Planning stop and wait maneuver"); - resp.new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage( - current_state.downtrack, traffic_light_down_track - config_.vehicle_length, current_state.speed, crossed_lanelets.front().id(), - crossed_lanelets.back().id(), current_state.stamp, - req.header.stamp + ros::Duration(config_.min_maneuver_planning_period), stopping_accel)); - } -} - -void WzStrategicPlugin::planWhenWAITING(const cav_srvs::PlanManeuversRequest& req, - cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - const std::vector& traffic_lights) -{ - if (traffic_lights.empty()) - { - throw std::invalid_argument("While in WAITING state, the traffic lights disappeared."); - } - - auto nearest_traffic_light = traffic_lights.front(); - - double traffic_light_down_track = - wm_->routeTrackPos(nearest_traffic_light->stopLine().front().front().basicPoint2d()).downtrack; - - ROS_DEBUG("traffic_light_down_track %f", traffic_light_down_track); - - auto current_light_state_optional = nearest_traffic_light->predictState(lanelet::time::timeFromSec(req.header.stamp.toSec())); - - if (!validLightState(current_light_state_optional, req.header.stamp)) - return; - - if (current_light_state_optional.get().second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED) - { - transition_table_.signal(TransitEvent::RED_TO_GREEN_LIGHT); // If the light is green send the light transition - // signal - return; - } - - // A fixed buffer to add to stopping maneuvers once the vehicle is already stopped to ensure that they can have their - // trajectory planned - constexpr double stop_maneuver_buffer = 10.0; - - // If the light is not green then continue waiting by creating a stop and wait maneuver ontop of the vehicle - double stopping_accel = config_.vehicle_decel_limit_multiplier * config_.vehicle_decel_limit; - - resp.new_plan.maneuvers.push_back(composeStopAndWaitManeuverMessage( - current_state.downtrack - stop_maneuver_buffer, traffic_light_down_track, current_state.speed, - current_state.lane_id, current_state.lane_id, current_state.stamp, - current_state.stamp + ros::Duration(config_.min_maneuver_planning_period), stopping_accel)); -} - -void WzStrategicPlugin::planWhenDEPARTING(const cav_srvs::PlanManeuversRequest& req, - cav_srvs::PlanManeuversResponse& resp, const VehicleState& current_state, - double intersection_end_downtrack, double intersection_speed_limit) -{ - if (current_state.downtrack > intersection_end_downtrack) - { - transition_table_.signal(TransitEvent::INTERSECTION_EXIT); // If we are past the most recent - return; - } - - // Calculate exit time assuming constant acceleration - ros::Time intersection_exit_time = - current_state.stamp + - ros::Duration(2.0 * (intersection_end_downtrack - current_state.downtrack) / (current_state.speed + intersection_speed_limit)); - - // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver - std::vector crossed_lanelets = - getLaneletsBetweenWithException(current_state.downtrack, intersection_end_downtrack, true, true); - - // Compose intersection transit maneuver - resp.new_plan.maneuvers.push_back(composeIntersectionTransitMessage( - current_state.downtrack, intersection_end_downtrack, current_state.speed, intersection_speed_limit, - current_state.stamp, intersection_exit_time, crossed_lanelets.front().id(), crossed_lanelets.back().id())); -} - -bool WzStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp) -{ - if (!wm_->getRoute()) - { - ROS_ERROR_STREAM("Could not plan maneuvers as route was not available"); - return true; - } - - ROS_DEBUG("Finding car information"); - - // Extract vehicle data from request - VehicleState current_state = extractInitialState(req); - if (transition_table_.getState() != TransitState::UNAVAILABLE && !req.prior_plan.maneuvers.empty()) - { - ROS_WARN_STREAM("State is NOT UNAVAILABLE AND Maneuvers in request is NOT empty"); - return true; - } - // Get current traffic light information - ROS_DEBUG("\n\nFinding traffic_light information"); - - auto traffic_list = wm_->getSignalsAlongRoute({ req.veh_x, req.veh_y }); - - TransitState prev_state; - - do - { - // Clear previous maneuvers planned by this plugin as guard against state change since each state generates an - // independant set of maneuvers - resp.new_plan = cav_msgs::ManeuverPlan(); - - prev_state = transition_table_.getState(); // Cache previous state to check if state has changed after 1 iteration - - /* NOTE: Leaving this commented out code is intentional to provide an easy way to monitor light state at runtime. If a better way is implemented then this can be removed - if (!traffic_list.empty()) { - auto nearest_traffic_light = traffic_list.front(); - ROS_ERROR_STREAM("\n\nCurrent Light State: " << nearest_traffic_light->getState().get() - << std::endl << " 1: " << nearest_traffic_light->predictState(lanelet::time::timeFromSec((ros::Time::now() + ros::Duration(1.0)).toSec())).get() - << std::endl << " 2: " << nearest_traffic_light->predictState(lanelet::time::timeFromSec((ros::Time::now() + ros::Duration(2.0)).toSec())).get() - << std::endl << " 3: " << nearest_traffic_light->predictState(lanelet::time::timeFromSec((ros::Time::now() + ros::Duration(3.0)).toSec())).get() - << std::endl << " 4: " << nearest_traffic_light->predictState(lanelet::time::timeFromSec((ros::Time::now() + ros::Duration(4.0)).toSec())).get() - << std::endl << " 5: " << nearest_traffic_light->predictState(lanelet::time::timeFromSec((ros::Time::now() + ros::Duration(5.0)).toSec())).get() - << std::endl); - } - */ - ROS_INFO_STREAM("Planning in state: " << transition_table_.getState()); - switch (transition_table_.getState()) - { - case TransitState::UNAVAILABLE: - planWhenUNAVAILABLE(req, resp, current_state, traffic_list); - break; - - case TransitState::APPROACHING: - planWhenAPPROACHING(req, resp, current_state, traffic_list); - break; - - case TransitState::WAITING: - planWhenWAITING(req, resp, current_state, traffic_list); - break; - - case TransitState::DEPARTING: - // Reset intersection state since we are no longer in an intersection - if (!intersection_end_downtrack_ || !intersection_speed_) - { - throw std::invalid_argument("State is DEPARTING but the intersection variables are not available"); - } - planWhenDEPARTING(req, resp, current_state, intersection_end_downtrack_.get(), intersection_speed_.get()); - break; - - default: - throw std::invalid_argument("WorkZone Strategic Plugin entered unknown state"); - break; - } - - } while (transition_table_.getState() != prev_state); // If the state has changed then continue looping - - return true; - // We need to evaluate the events so the state transitions can be triggered -} - -cav_msgs::Maneuver WzStrategicPlugin::composeLaneFollowingManeuverMessage(double start_dist, double end_dist, - double start_speed, double target_speed, - ros::Time start_time, ros::Time end_time, - std::vector lane_ids) const -{ - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING; - maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; - maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = config_.lane_following_plugin_name; - maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name; - maneuver_msg.lane_following_maneuver.start_dist = start_dist; - maneuver_msg.lane_following_maneuver.start_speed = start_speed; - maneuver_msg.lane_following_maneuver.end_dist = end_dist; - maneuver_msg.lane_following_maneuver.end_speed = target_speed; - maneuver_msg.lane_following_maneuver.start_time = start_time; - maneuver_msg.lane_following_maneuver.end_time = end_time; - maneuver_msg.lane_following_maneuver.lane_ids = - lanelet::utils::transform(lane_ids, [](auto id) { return std::to_string(id); }); - - ROS_INFO_STREAM("Creating lane follow start dist: " << start_dist << " end dist: " << end_dist); - - return maneuver_msg; -} - -cav_msgs::Maneuver WzStrategicPlugin::composeStopAndWaitManeuverMessage(double current_dist, double end_dist, - double start_speed, - const lanelet::Id& starting_lane_id, - const lanelet::Id& ending_lane_id, - ros::Time start_time, ros::Time end_time, double stopping_accel) const -{ - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::STOP_AND_WAIT; - maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; - maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector = - cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN | cav_msgs::ManeuverParameters::HAS_FLOAT_META_DATA; - maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = config_.stop_and_wait_plugin_name; - maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = config_.strategic_plugin_name; - maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist; - maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist; - maneuver_msg.stop_and_wait_maneuver.start_speed = start_speed; - maneuver_msg.stop_and_wait_maneuver.start_time = start_time; - maneuver_msg.stop_and_wait_maneuver.end_time = end_time; - maneuver_msg.stop_and_wait_maneuver.starting_lane_id = std::to_string(starting_lane_id); - maneuver_msg.stop_and_wait_maneuver.ending_lane_id = std::to_string(ending_lane_id); - - // Set the meta data for the stop location buffer - maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(config_.stopping_location_buffer); - maneuver_msg.stop_and_wait_maneuver.parameters.float_valued_meta_data.push_back(stopping_accel); - - ROS_INFO_STREAM("Creating stop and wait start dist: " << current_dist << " end dist: " << end_dist); - - return maneuver_msg; -} - -cav_msgs::Maneuver WzStrategicPlugin::composeIntersectionTransitMessage(double start_dist, double end_dist, - double start_speed, double target_speed, - ros::Time start_time, ros::Time end_time, - const lanelet::Id& starting_lane_id, - const lanelet::Id& ending_lane_id) const -{ - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT; - maneuver_msg.intersection_transit_straight_maneuver.parameters.negotiation_type = - cav_msgs::ManeuverParameters::NO_NEGOTIATION; - maneuver_msg.intersection_transit_straight_maneuver.parameters.presence_vector = - cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin = - config_.intersection_transit_plugin_name; - maneuver_msg.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin = - config_.strategic_plugin_name; - maneuver_msg.intersection_transit_straight_maneuver.start_dist = start_dist; - maneuver_msg.intersection_transit_straight_maneuver.start_speed = start_speed; - maneuver_msg.intersection_transit_straight_maneuver.start_time = start_time; - maneuver_msg.intersection_transit_straight_maneuver.end_dist = end_dist; - maneuver_msg.intersection_transit_straight_maneuver.end_speed = target_speed; - maneuver_msg.intersection_transit_straight_maneuver.end_time = end_time; - maneuver_msg.intersection_transit_straight_maneuver.starting_lane_id = std::to_string(starting_lane_id); - maneuver_msg.intersection_transit_straight_maneuver.ending_lane_id = std::to_string(ending_lane_id); - - // Start time and end time for maneuver are assigned in updateTimeProgress - - ROS_INFO_STREAM("Creating IntersectionTransitManeuver start dist: " << start_dist << " end dist: " << end_dist - << " From lanelet: " << starting_lane_id - << " to lanelet: " << ending_lane_id); - - return maneuver_msg; -} - -double WzStrategicPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt) const -{ - lanelet::Optional traffic_rules = wm_->getTrafficRules(); - if (traffic_rules) - { - return (*traffic_rules)->speedLimit(llt).speedLimit.value(); - } - else - { - throw std::invalid_argument("Valid traffic rules object could not be built"); - } -} - -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/wz_strategic_plugin/test/test_fixture.h b/wz_strategic_plugin/test/test_fixture.h deleted file mode 100644 index c185949e96..0000000000 --- a/wz_strategic_plugin/test/test_fixture.h +++ /dev/null @@ -1,66 +0,0 @@ -#pragma once -/* - * Copyright (C) 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 - -// Unit tests for transition table -namespace wz_strategic_plugin -{ -/** - * \brief TODO - */ -class WorkZoneTestFixture : public ::testing::Test -{ - /** - * - getGuidanceTestMap gives a simple one way, 3 lane map (25mph speed limit) with one static prebaked obstacle and - * 4 lanelets in a lane (if 2 stripes make up one lanelet): - * - * |1203|1213|1223| - * | _ _ _ _ _| - * |1202| Ob |1222| - * | _ _ _ _ _| - * |1201|1211|1221| num = lanelet id hardcoded for easier testing - * | _ _ _ _ _| | = lane lines - * |1200|1210|1220| - - - = Lanelet boundary - * | | O = Default Obstacle - * **************** - * START_LINE - */ - -protected: - void SetUp() override - { - carma_wm::test::MapOptions options; - options.lane_length_ = 25; - options.lane_width_ = 3.7; - options.speed_limit_ = carma_wm::test::MapOptions::SpeedLimit::DEFAULT; - options.obstacle_ = carma_wm::test::MapOptions::Obstacle::NONE; - - cmw_ = carma_wm::test::getGuidanceTestMap(options); - - carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, cmw_); - } - - // void TearDown() override {} - - std::shared_ptr cmw_; -}; - -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/wz_strategic_plugin/test/test_main.cpp b/wz_strategic_plugin/test/test_main.cpp deleted file mode 100644 index 49fba5c9c2..0000000000 --- a/wz_strategic_plugin/test/test_main.cpp +++ /dev/null @@ -1,30 +0,0 @@ -/* - * Copyright (C) 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 - -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::Time::init(); - ROSCONSOLE_AUTOINIT; - if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) { - ros::console::notifyLoggerLevelsChanged(); - } - auto res = RUN_ALL_TESTS(); - return res; -} \ No newline at end of file diff --git a/wz_strategic_plugin/test/test_strategic_plugin.cpp b/wz_strategic_plugin/test/test_strategic_plugin.cpp deleted file mode 100644 index f1c2f4e15d..0000000000 --- a/wz_strategic_plugin/test/test_strategic_plugin.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* - * Copyright (C) 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 "test_fixture.h" -#include "wz_strategic_plugin/wz_strategic_plugin.h" -#include "wz_strategic_plugin/wz_strategic_plugin_config.h" - -// Unit tests for strategic plugin -namespace wz_strategic_plugin -{ - -/** - * - getGuidanceTestMap gives a simple one way, 3 lane map (25mph speed limit) with one static prebaked obstacle and - * 4 lanelets in a lane (if 2 stripes make up one lanelet): - * - * |1203|1213|1223| - * | _ _ _ _ _| - * |1202| Ob |1222| - * | _ _ _ _ _| - * |1201|1211|1221| num = lanelet id hardcoded for easier testing - * | _ _ _ _ _| | = lane lines - * |1200|1210|1220| - - - = Lanelet boundary - * | | O = Default Obstacle - * **************** - * START_LINE - */ -TEST_F(WorkZoneTestFixture, planManeuverCb) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - cav_srvs::PlanManeuversRequest req; - cav_srvs::PlanManeuversResponse resp; - - // Light will be located on lanelet 1200 (25m) and control lanelet 1202, 1203 - lanelet::Id traffic_light_id = lanelet::utils::getId(); - carma_wm::test::addTrafficLight(cmw_, traffic_light_id, {1201}, { 1202 }); - - ROS_DEBUG("Out of range test "); - req.header.stamp = ros::Time(0); - req.veh_x = 1.85; - req.veh_y = 1.0; // Out of range of light which should kick in at 8.36567466667 - req.veh_downtrack = req.veh_y; - req.veh_logitudinal_velocity = 11.176; - req.veh_lane_id = "1200"; - - wzp.planManeuverCb(req, resp); - ROS_ERROR_STREAM("resp.newplanmaneuvers size: "< -#include -#include "test_fixture.h" -#include "wz_strategic_plugin/wz_strategic_plugin.h" -#include "wz_strategic_plugin/wz_strategic_plugin_config.h" - -// Unit tests for strategic plugin helper methods -namespace wz_strategic_plugin -{ -TEST_F(WorkZoneTestFixture, getDiscoveryMsg) -{ - WzStrategicPluginConfig config; - config.strategic_plugin_name = "test name"; - WzStrategicPlugin wzp(cmw_, config); - - auto msg = wzp.getDiscoveryMsg(); - ASSERT_TRUE(msg.name.compare("test name") == 0); - ASSERT_TRUE(msg.version_id.compare("v1.0") == 0); - ASSERT_TRUE(msg.available); - ASSERT_TRUE(msg.activated); - ASSERT_EQ(msg.type, cav_msgs::Plugin::STRATEGIC); - ASSERT_TRUE(msg.capability.compare("strategic_plan/plan_maneuvers") == 0); -} - -TEST_F(WorkZoneTestFixture, supportedLightState) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED)); - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE)); - ASSERT_TRUE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); - - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::UNAVAILABLE)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::DARK)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PRE_MOVEMENT)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE)); - ASSERT_FALSE(wzp.supportedLightState(lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC)); -} - -TEST_F(WorkZoneTestFixture, estimate_distance_to_stop) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - ASSERT_NEAR(wzp.estimate_distance_to_stop(8.5, 1.3), 27.788461538461537, 0.00001); -} - -TEST_F(WorkZoneTestFixture, estimate_time_to_stop) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - ASSERT_NEAR(wzp.estimate_time_to_stop(27.788461538461537, 8.5), 6.538461538461538, 0.00001); -} - -TEST_F(WorkZoneTestFixture, extractInitialState) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - cav_srvs::PlanManeuversRequest req; - req.header.stamp = ros::Time(5.0); - req.veh_downtrack = 23.5; - req.veh_logitudinal_velocity = 10.2; - req.veh_lane_id = "1002"; - - WzStrategicPlugin::VehicleState result = wzp.extractInitialState(req); - - ASSERT_EQ(req.header.stamp.toSec(), result.stamp.toSec()); - ASSERT_EQ(req.veh_downtrack, result.downtrack); - ASSERT_EQ(req.veh_logitudinal_velocity, result.speed); - ASSERT_TRUE(req.veh_lane_id.compare(std::to_string(result.lane_id)) == 0); -} - -TEST_F(WorkZoneTestFixture, validLightState) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - boost::posix_time::ptime dummy_time; - ASSERT_TRUE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED), ros::Time(1))); - ASSERT_TRUE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE), ros::Time(1))); - ASSERT_TRUE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN), ros::Time(1))); - - ASSERT_FALSE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::UNAVAILABLE), ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::DARK), ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::STOP_THEN_PROCEED), ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::PRE_MOVEMENT), ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::PERMISSIVE_MOVEMENT_ALLOWED), ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::PERMISSIVE_CLEARANCE), ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(std::pair(dummy_time, lanelet::CarmaTrafficSignalState::CAUTION_CONFLICTING_TRAFFIC), ros::Time(1))); - ASSERT_FALSE(wzp.validLightState(boost::none, ros::Time(1))); -} - -TEST_F(WorkZoneTestFixture, getLaneletsBetweenWithException) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - auto result = wzp.getLaneletsBetweenWithException(24, 26, true); - - ASSERT_EQ(2, result.size()); - ASSERT_EQ(1200, result[0].id()); - ASSERT_EQ(1201, result[1].id()); - - result = wzp.getLaneletsBetweenWithException(24, 24, true); - - ASSERT_EQ(1, result.size()); - ASSERT_EQ(1200, result[0].id()); - - ASSERT_THROW(wzp.getLaneletsBetweenWithException(24, 23, true), std::invalid_argument); -} - -TEST_F(WorkZoneTestFixture, composeLaneFollowingManeuverMessage) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - auto result = - wzp.composeLaneFollowingManeuverMessage(10.2, 20.4, 5, 10, ros::Time(1.2), ros::Time(2.2), { 1200, 1201 }); - - ASSERT_EQ(cav_msgs::Maneuver::LANE_FOLLOWING, result.type); - ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, result.lane_following_maneuver.parameters.negotiation_type); - ASSERT_EQ(cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN, - result.lane_following_maneuver.parameters.presence_vector); - ASSERT_TRUE(config.lane_following_plugin_name.compare( - result.lane_following_maneuver.parameters.planning_tactical_plugin) == 0); - ASSERT_TRUE( - config.strategic_plugin_name.compare(result.lane_following_maneuver.parameters.planning_strategic_plugin) == 0); - - ASSERT_EQ(10.2, result.lane_following_maneuver.start_dist); - ASSERT_EQ(20.4, result.lane_following_maneuver.end_dist); - ASSERT_EQ(5, result.lane_following_maneuver.start_speed); - ASSERT_EQ(10, result.lane_following_maneuver.end_speed); - ASSERT_EQ(ros::Time(1.2), result.lane_following_maneuver.start_time); - ASSERT_EQ(ros::Time(2.2), result.lane_following_maneuver.end_time); - ASSERT_EQ(2, result.lane_following_maneuver.lane_ids.size()); - ASSERT_TRUE(result.lane_following_maneuver.lane_ids[0].compare("1200") == 0); - ASSERT_TRUE(result.lane_following_maneuver.lane_ids[1].compare("1201") == 0); -} - -TEST_F(WorkZoneTestFixture, composeStopAndWaitManeuverMessage) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - double stopping_acceleration = 1.0; - auto result = wzp.composeStopAndWaitManeuverMessage(10.2, 20.4, 5, 1200, 1201, ros::Time(1.2), ros::Time(2.2), stopping_acceleration); - - ASSERT_EQ(cav_msgs::Maneuver::STOP_AND_WAIT, result.type); - ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, result.stop_and_wait_maneuver.parameters.negotiation_type); - ASSERT_EQ(cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN | cav_msgs::ManeuverParameters::HAS_FLOAT_META_DATA, - result.stop_and_wait_maneuver.parameters.presence_vector); // TODO diff - ASSERT_TRUE(config.stop_and_wait_plugin_name.compare( - result.stop_and_wait_maneuver.parameters.planning_tactical_plugin) == 0); - ASSERT_TRUE( - config.strategic_plugin_name.compare(result.stop_and_wait_maneuver.parameters.planning_strategic_plugin) == 0); - - ASSERT_EQ(10.2, result.stop_and_wait_maneuver.start_dist); - ASSERT_EQ(20.4, result.stop_and_wait_maneuver.end_dist); - ASSERT_EQ(5, result.stop_and_wait_maneuver.start_speed); - ASSERT_EQ(ros::Time(1.2), result.stop_and_wait_maneuver.start_time); - ASSERT_EQ(ros::Time(2.2), result.stop_and_wait_maneuver.end_time); - ASSERT_TRUE(result.stop_and_wait_maneuver.starting_lane_id.compare("1200") == 0); - ASSERT_TRUE(result.stop_and_wait_maneuver.ending_lane_id.compare("1201") == 0); -} - -TEST_F(WorkZoneTestFixture, composeIntersectionTransitMessage) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - auto result = wzp.composeIntersectionTransitMessage(10.2, 20.4, 5, 10, ros::Time(1.2), ros::Time(2.2), 1200, 1201); - - ASSERT_EQ(cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT, result.type); - ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, - result.intersection_transit_straight_maneuver.parameters.negotiation_type); - ASSERT_EQ(cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN, - result.intersection_transit_straight_maneuver.parameters.presence_vector); - ASSERT_TRUE(config.intersection_transit_plugin_name.compare( - result.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin) == 0); - ASSERT_TRUE(config.strategic_plugin_name.compare( - result.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin) == 0); - - ASSERT_EQ(10.2, result.intersection_transit_straight_maneuver.start_dist); - ASSERT_EQ(20.4, result.intersection_transit_straight_maneuver.end_dist); - ASSERT_EQ(5, result.intersection_transit_straight_maneuver.start_speed); - ASSERT_EQ(10, result.intersection_transit_straight_maneuver.end_speed); - ASSERT_EQ(ros::Time(1.2), result.intersection_transit_straight_maneuver.start_time); - ASSERT_EQ(ros::Time(2.2), result.intersection_transit_straight_maneuver.end_time); - ASSERT_TRUE(result.intersection_transit_straight_maneuver.starting_lane_id.compare("1200") == 0); - ASSERT_TRUE(result.intersection_transit_straight_maneuver.ending_lane_id.compare("1201") == 0); -} - -TEST_F(WorkZoneTestFixture, findSpeedLimit) -{ - WzStrategicPluginConfig config; - WzStrategicPlugin wzp(cmw_, config); - - auto ll_iterator = cmw_->getMap()->laneletLayer.find(1200); - if (ll_iterator == cmw_->getMap()->laneletLayer.end()) - FAIL() << "Expected lanelet not present in map. Unit test may not be structured correctly"; - - ASSERT_NEAR(11.176, wzp.findSpeedLimit(*ll_iterator), 0.00001); -} - -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/wz_strategic_plugin/test/test_transition_table.cpp b/wz_strategic_plugin/test/test_transition_table.cpp deleted file mode 100644 index bf7bfcad92..0000000000 --- a/wz_strategic_plugin/test/test_transition_table.cpp +++ /dev/null @@ -1,122 +0,0 @@ -/* - * Copyright (C) 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 "test_fixture.h" -#include "wz_strategic_plugin/wz_state_transition_table.h" - -// Unit tests for transition table -namespace wz_strategic_plugin -{ -/** - * \brief Helper function to assert that the provided list of signals do not change the current state of the provided - * transition table - */ -void testNonTransitionSignals(WorkZoneStateTransitionTable& table, const std::vector& signals) -{ - TransitState initial_state = table.getState(); - for (TransitEvent signal : signals) - { - table.signal(signal); - ASSERT_EQ(initial_state, table.getState()); - } -} - -TEST_F(WorkZoneTestFixture, getState) -{ - WorkZoneStateTransitionTable table; - ASSERT_EQ(TransitState::UNAVAILABLE, table.getState()); - - table.signal(TransitEvent::IN_STOPPING_RANGE); - ASSERT_EQ(TransitState::APPROACHING, table.getState()); -} - -// Unit test to evaluate all state transitions -TEST_F(WorkZoneTestFixture, signal) -{ - WorkZoneStateTransitionTable table; - ASSERT_EQ(TransitState::UNAVAILABLE, table.getState()); - - testNonTransitionSignals(table, { TransitEvent::STOPPED, TransitEvent::RED_TO_GREEN_LIGHT, - TransitEvent::CROSSED_STOP_BAR, TransitEvent::INTERSECTION_EXIT }); - - table.signal(TransitEvent::IN_STOPPING_RANGE); - ASSERT_EQ(TransitState::APPROACHING, table.getState()); - - testNonTransitionSignals( - table, { TransitEvent::IN_STOPPING_RANGE, TransitEvent::RED_TO_GREEN_LIGHT, TransitEvent::INTERSECTION_EXIT }); - - table.signal(TransitEvent::STOPPED); - - ASSERT_EQ(TransitState::WAITING, table.getState()); - - testNonTransitionSignals(table, { TransitEvent::IN_STOPPING_RANGE, TransitEvent::STOPPED, - TransitEvent::CROSSED_STOP_BAR, TransitEvent::INTERSECTION_EXIT }); - - table.signal(TransitEvent::RED_TO_GREEN_LIGHT); - - ASSERT_EQ(TransitState::DEPARTING, table.getState()); - - testNonTransitionSignals(table, { TransitEvent::IN_STOPPING_RANGE, TransitEvent::STOPPED, - TransitEvent::RED_TO_GREEN_LIGHT, TransitEvent::CROSSED_STOP_BAR }); - - table.signal(TransitEvent::INTERSECTION_EXIT); - - ASSERT_EQ(TransitState::UNAVAILABLE, table.getState()); - - // Reset table to test non-stopping case - table = WorkZoneStateTransitionTable(); - - table.signal(TransitEvent::IN_STOPPING_RANGE); - - ASSERT_EQ(TransitState::APPROACHING, table.getState()); - - table.signal(TransitEvent::CROSSED_STOP_BAR); - - ASSERT_EQ(TransitState::DEPARTING, table.getState()); -} - -TEST_F(WorkZoneTestFixture, setTransitionCallback) -{ - WorkZoneStateTransitionTable table; - - boost::optional prev, current; - boost::optional sig; - table.setTransitionCallback([&](TransitState prev_state, TransitState new_state, TransitEvent signal) { - prev = prev_state; - current = new_state; - sig = signal; - }); - - ASSERT_FALSE(!!prev); - ASSERT_FALSE(!!current); - ASSERT_FALSE(!!sig); - - ASSERT_EQ(TransitState::UNAVAILABLE, table.getState()); - - table.signal(TransitEvent::IN_STOPPING_RANGE); - - ASSERT_EQ(TransitState::APPROACHING, table.getState()); - ASSERT_TRUE(!!prev); - ASSERT_TRUE(!!current); - ASSERT_TRUE(!!sig); - - ASSERT_EQ(TransitState::UNAVAILABLE, prev.get()); - ASSERT_EQ(TransitState::APPROACHING, current.get()); - ASSERT_EQ(TransitEvent::IN_STOPPING_RANGE, sig.get()); -} -} // namespace wz_strategic_plugin \ No newline at end of file diff --git a/yield_plugin/include/yield_plugin/yield_plugin_node.h b/yield_plugin/include/yield_plugin/yield_plugin_node.h index d1e24f6020..09a1a18c85 100644 --- a/yield_plugin/include/yield_plugin/yield_plugin_node.h +++ b/yield_plugin/include/yield_plugin/yield_plugin_node.h @@ -76,7 +76,7 @@ class YieldPluginNode YieldPlugin worker(wm_, config, [&discovery_pub](auto msg) { discovery_pub.publish(msg); }, [&mob_resp_pub](auto msg) { mob_resp_pub.publish(msg); }, [&lc_status_pub](auto msg) { lc_status_pub.publish(msg); }); - ros::ServiceServer trajectory_srv_ = nh.advertiseService("plugins/YieldPlugin/plan_trajectory", + ros::ServiceServer trajectory_srv_ = nh.advertiseService("yield_plugin/plan_trajectory", &YieldPlugin::plan_trajectory_cb, &worker); ros::Subscriber mob_request_sub = nh.subscribe("incoming_mobility_request", 5, &YieldPlugin::mobilityrequest_cb, &worker); ros::Subscriber bsm_sub = nh.subscribe("bsm_outbound", 1, &YieldPlugin::bsm_cb, &worker); diff --git a/yield_plugin/src/yield_plugin.cpp b/yield_plugin/src/yield_plugin.cpp index cad4215ccf..d9c6a7d5d8 100644 --- a/yield_plugin/src/yield_plugin.cpp +++ b/yield_plugin/src/yield_plugin.cpp @@ -44,10 +44,10 @@ namespace yield_plugin : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher), mobility_response_publisher_(mobility_response_publisher), lc_status_publisher_(lc_status_publisher) { - plugin_discovery_msg_.name = "YieldPlugin"; + plugin_discovery_msg_.name = "yield_plugin"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = false; + plugin_discovery_msg_.activated = true; plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; } diff --git a/yield_plugin/test/test_yield_plugin.cpp b/yield_plugin/test/test_yield_plugin.cpp index efd48fdde2..e2c6ffd239 100644 --- a/yield_plugin/test/test_yield_plugin.cpp +++ b/yield_plugin/test/test_yield_plugin.cpp @@ -98,7 +98,7 @@ TEST(YieldPlugin, UnitTestYield) ros::CARMANodeHandle nh; - ros::ServiceClient plugin1= nh.serviceClient("plugins/YieldPlugin/plan_trajectory"); + ros::ServiceClient plugin1= nh.serviceClient("yield_plugin/plan_trajectory"); ros::Subscriber mob_resp_sub = nh.subscribe("outgoing_mobility_response", 5, callback); ros::Subscriber lc_status_sub = nh.subscribe("cooperative_lane_change_status", 5, status_callback); From 9ec2a2ec21656725185b908cad5c3194cfccfee5 Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Thu, 7 Jul 2022 16:20:44 -0400 Subject: [PATCH 030/165] updated readme with dockerhub build status checks --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 6847427f7a..6ad352cfdd 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ -| CicleCI Build Status | Sonar Code Quality | -|----------------------|---------------------| -[![CircleCI](https://circleci.com/gh/usdot-fhwa-stol/carma-platform.svg?style=svg)](https://circleci.com/gh/usdot-fhwa-stol/carma-platform) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=usdot-fhwa-stol_CARMAPlatform&metric=alert_status)](https://sonarcloud.io/dashboard?id=usdot-fhwa-stol_CARMAPlatform) | +| CicleCI Build Status | Sonar Code Quality | DockerHub Build Dev | DockerHub Candidate | DockerHub STOL | +|------|-----|-----|-----|-----| +[![CircleCI](https://circleci.com/gh/usdot-fhwa-stol/carma-platform.svg?style=svg)](https://circleci.com/gh/usdot-fhwa-stol/carma-platform) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=usdot-fhwa-stol_CARMAPlatform&metric=alert_status)](https://sonarcloud.io/dashboard?id=usdot-fhwa-stol_CARMAPlatform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/carma-platform?label=Usdotfhwastol%20DEV&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastoldev/carma-platform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/carma-platform?label=Usdotfhwastol%20Candidate&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/carma-platform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/carma-platform?label=Usdotfhwa%20STOL&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastol/carma-platform) # CARMA ![CARMA Arch](docs/image/CARMA3_Vehicles.jpg) From f55c4910af7e97ce3a8f451cc0c2387531fbdf3f Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Fri, 8 Jul 2022 11:49:07 -0400 Subject: [PATCH 031/165] Updated the Dockhub org's names order --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 6ad352cfdd..674f9ab14e 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ -| CicleCI Build Status | Sonar Code Quality | DockerHub Build Dev | DockerHub Candidate | DockerHub STOL | +| CicleCI Build Status | Sonar Code Quality | DockerHub Release | DockerHub Release Candidate | DockerHub Develop | |------|-----|-----|-----|-----| -[![CircleCI](https://circleci.com/gh/usdot-fhwa-stol/carma-platform.svg?style=svg)](https://circleci.com/gh/usdot-fhwa-stol/carma-platform) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=usdot-fhwa-stol_CARMAPlatform&metric=alert_status)](https://sonarcloud.io/dashboard?id=usdot-fhwa-stol_CARMAPlatform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/carma-platform?label=Usdotfhwastol%20DEV&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastoldev/carma-platform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/carma-platform?label=Usdotfhwastol%20Candidate&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/carma-platform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/carma-platform?label=Usdotfhwa%20STOL&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastol/carma-platform) +[![CircleCI](https://circleci.com/gh/usdot-fhwa-stol/carma-platform.svg?style=svg)](https://circleci.com/gh/usdot-fhwa-stol/carma-platform) | [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=usdot-fhwa-stol_CARMAPlatform&metric=alert_status)](https://sonarcloud.io/dashboard?id=usdot-fhwa-stol_CARMAPlatform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastol/carma-platform?label=Usdotfhwa%20STOL&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastol/carma-platform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastolcandidate/carma-platform?label=Usdotfhwastol%20Candidate&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastolcandidate/carma-platform) | [![Docker Cloud Build Status](https://img.shields.io/docker/cloud/build/usdotfhwastoldev/carma-platform?label=Usdotfhwastol%20DEV&logo=%232496ED)](https://hub.docker.com/repository/docker/usdotfhwastoldev/carma-platform) # CARMA ![CARMA Arch](docs/image/CARMA3_Vehicles.jpg) From fcbe603f90b2c08e1ca1602bef885cf6cacaebf6 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Wed, 13 Jul 2022 12:28:37 -0400 Subject: [PATCH 032/165] Feature/ilc_ros2 and guidance_controller fix (#1843) Description Convert InLaneCruisingPlugin to ROS2. This PR also includes a fix for guidance_controller where it was mistakenly trying to configure ROS1 nodes Related Issue #1842 Motivation and Context See above. How Has This Been Tested? Local integration tested --- .../basic_autonomy/smoothing/BSpline.h | 2 +- .../basic_autonomy/smoothing/SplineI.h | 2 +- basic_autonomy/src/smoothing/BSpline.cpp | 2 +- .../basic_autonomy_ros2/smoothing/BSpline.hpp | 2 +- .../basic_autonomy_ros2/smoothing/SplineI.hpp | 2 +- basic_autonomy_ros2/src/smoothing/BSpline.cpp | 2 +- carma/launch/guidance.launch | 6 - carma/launch/guidance.launch.py | 31 +- carma/launch/plugins.launch | 2 +- carma/launch/plugins.launch.py | 84 +++ .../plugin_base_node.hpp | 15 + .../src/plugin_base_node.cpp | 12 +- .../src/tactical_plugin.cpp | 2 +- inlanecruising_plugin/CMakeLists.txt | 125 ++-- inlanecruising_plugin/config/parameters.yaml | 2 +- ...ing_config.h => inlanecruising_config.hpp} | 2 +- .../inlanecruising_plugin.h | 112 --- .../inlanecruising_plugin.hpp | 115 +++ .../inlanecruising_plugin_node.h | 102 --- .../inlanecruising_plugin_node.hpp | 83 +++ .../log/{log.h => log.hpp} | 12 +- .../smoothing/{BSpline.h => BSpline.hpp} | 8 +- .../smoothing/{SplineI.h => SplineI.hpp} | 6 +- .../smoothing/{filters.h => filters.hpp} | 2 +- .../launch/inlanecruising_plugin.launch | 20 - .../launch/inlanecruising_plugin.launch.py | 68 ++ inlanecruising_plugin/package.xml | 39 +- .../src/inlanecruising_plugin.cpp | 134 ++-- .../src/inlanecruising_plugin_node.cpp | 154 ++++ inlanecruising_plugin/src/main.cpp | 24 +- .../src/smoothing/BSpline.cpp | 4 +- inlanecruising_plugin/test/helper.hpp | 45 ++ inlanecruising_plugin/test/ilc_plugin.test | 27 - inlanecruising_plugin/test/rostest_ilc.cpp | 97 +-- .../test/test_inlanecruising_plugin.cpp | 670 ++---------------- .../test_inlanecruising_plugin_planning.cpp | 132 ++-- .../config/guidance_controller_config.yaml | 14 +- .../guidance_controller/entry.h | 4 +- .../guidance_controller_config.hpp | 10 +- .../guidance_controller/plugin_manager.h | 6 + .../guidance_controller.cpp | 7 +- .../guidance_controller/plugin_manager.cpp | 25 +- .../test/test_plugin_manager.cpp | 2 + 43 files changed, 999 insertions(+), 1216 deletions(-) create mode 100644 carma/launch/plugins.launch.py rename inlanecruising_plugin/include/inlanecruising_plugin/{inlanecruising_config.h => inlanecruising_config.hpp} (99%) delete mode 100644 inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.h create mode 100644 inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin.hpp delete mode 100644 inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.h create mode 100644 inlanecruising_plugin/include/inlanecruising_plugin/inlanecruising_plugin_node.hpp rename inlanecruising_plugin/include/inlanecruising_plugin/log/{log.h => log.hpp} (68%) rename inlanecruising_plugin/include/inlanecruising_plugin/smoothing/{BSpline.h => BSpline.hpp} (85%) rename inlanecruising_plugin/include/inlanecruising_plugin/smoothing/{SplineI.h => SplineI.hpp} (94%) rename inlanecruising_plugin/include/inlanecruising_plugin/smoothing/{filters.h => filters.hpp} (98%) delete mode 100644 inlanecruising_plugin/launch/inlanecruising_plugin.launch create mode 100644 inlanecruising_plugin/launch/inlanecruising_plugin.launch.py create mode 100644 inlanecruising_plugin/src/inlanecruising_plugin_node.cpp create mode 100644 inlanecruising_plugin/test/helper.hpp delete mode 100644 inlanecruising_plugin/test/ilc_plugin.test diff --git a/basic_autonomy/include/basic_autonomy/smoothing/BSpline.h b/basic_autonomy/include/basic_autonomy/smoothing/BSpline.h index 4bc565dc48..5e3c1db6c6 100644 --- a/basic_autonomy/include/basic_autonomy/smoothing/BSpline.h +++ b/basic_autonomy/include/basic_autonomy/smoothing/BSpline.h @@ -34,7 +34,7 @@ class BSpline : public SplineI { public: ~BSpline(){}; - void setPoints(std::vector points) override; + void setPoints(const std::vector& points) override; lanelet::BasicPoint2d operator()(double t) const override; lanelet::BasicPoint2d first_deriv(double t) const override; lanelet::BasicPoint2d second_deriv(double t) const override; diff --git a/basic_autonomy/include/basic_autonomy/smoothing/SplineI.h b/basic_autonomy/include/basic_autonomy/smoothing/SplineI.h index 0aeba8c1f6..b8ee141a88 100644 --- a/basic_autonomy/include/basic_autonomy/smoothing/SplineI.h +++ b/basic_autonomy/include/basic_autonomy/smoothing/SplineI.h @@ -40,7 +40,7 @@ class SplineI * * \param points The key points */ - virtual void setPoints(std::vector points) = 0; + virtual void setPoints(const std::vector& points) = 0; /** * \brief Get the BasicPoint2d coordinate along the curve at t-th step. diff --git a/basic_autonomy/src/smoothing/BSpline.cpp b/basic_autonomy/src/smoothing/BSpline.cpp index 222dbbca6b..0c48e6947a 100644 --- a/basic_autonomy/src/smoothing/BSpline.cpp +++ b/basic_autonomy/src/smoothing/BSpline.cpp @@ -22,7 +22,7 @@ namespace basic_autonomy { namespace smoothing { -void BSpline::setPoints(std::vector points) +void BSpline::setPoints(const std::vector& points) { Eigen::MatrixXd matrix_points(2, points.size()); int row_index = 0; diff --git a/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/BSpline.hpp b/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/BSpline.hpp index 1b55d4da66..b03a5815dd 100644 --- a/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/BSpline.hpp +++ b/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/BSpline.hpp @@ -34,7 +34,7 @@ class BSpline : public SplineI { public: ~BSpline(){}; - void setPoints(std::vector points) override; + void setPoints(const std::vector& points) override; lanelet::BasicPoint2d operator()(double t) const override; lanelet::BasicPoint2d first_deriv(double t) const override; lanelet::BasicPoint2d second_deriv(double t) const override; diff --git a/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/SplineI.hpp b/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/SplineI.hpp index a214ba96f0..4f064cb2ef 100644 --- a/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/SplineI.hpp +++ b/basic_autonomy_ros2/include/basic_autonomy_ros2/smoothing/SplineI.hpp @@ -40,7 +40,7 @@ class SplineI * * \param points The key points */ - virtual void setPoints(std::vector points) = 0; + virtual void setPoints(const std::vector& points) = 0; /** * \brief Get the BasicPoint2d coordinate along the curve at t-th step. diff --git a/basic_autonomy_ros2/src/smoothing/BSpline.cpp b/basic_autonomy_ros2/src/smoothing/BSpline.cpp index 034d09599b..5d446854b8 100644 --- a/basic_autonomy_ros2/src/smoothing/BSpline.cpp +++ b/basic_autonomy_ros2/src/smoothing/BSpline.cpp @@ -21,7 +21,7 @@ namespace basic_autonomy { namespace smoothing { -void BSpline::setPoints(std::vector points) +void BSpline::setPoints(const std::vector& points) { Eigen::MatrixXd matrix_points(2, points.size()); int row_index = 0; diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 645ef6be74..25c1d1be81 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -144,12 +144,6 @@ - - - - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index b2277e2e15..c9bab1eccc 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -19,6 +19,7 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch.substitutions import EnvironmentVariable +from launch.substitutions import ThisLaunchFileDir from carma_ros2_utils.launch.get_log_level import GetLogLevel from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace from launch.substitutions import LaunchConfiguration @@ -30,6 +31,8 @@ from launch.actions import GroupAction from launch_ros.actions import set_remap from launch.actions import DeclareLaunchArgument +from launch_ros.actions import PushRosNamespace + # Launch file for launching the nodes in the CARMA guidance stack @@ -41,7 +44,7 @@ def generate_launch_description(): strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate') tactical_plugins_to_validate = LaunchConfiguration('tactical_plugins_to_validate') control_plugins_to_validate = LaunchConfiguration('control_plugins_to_validate') - + vehicle_config_dir = LaunchConfiguration('vehicle_config_dir') vehicle_config_param_file = LaunchConfiguration('vehicle_config_param_file') declare_vehicle_config_param_file_arg = DeclareLaunchArgument( name = 'vehicle_config_param_file', @@ -70,6 +73,9 @@ def generate_launch_description(): plan_delegator_param_file = os.path.join( get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml') + inlanecruising_plugin_file_path = os.path.join( + get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml') + env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') subsystem_controller_param_file = LaunchConfiguration('subsystem_controller_param_file') @@ -200,10 +206,30 @@ def generate_launch_description(): parameters=[ guidance_param_file ] - ) + ), ] ) + # Launch plugins + plugins_group = GroupAction( + actions=[ + PushRosNamespace("plugins"), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/plugins.launch.py']), + launch_arguments={ + 'route_file_folder' : route_file_folder, + 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file, + 'vehicle_config_param_file' : vehicle_config_param_file, + 'enable_guidance_plugin_validator' : enable_guidance_plugin_validator, + 'strategic_plugins_to_validate' : strategic_plugins_to_validate, + 'tactical_plugins_to_validate' : tactical_plugins_to_validate, + 'control_plugins_to_validate' : control_plugins_to_validate, + 'subsystem_controller_param_file' : [vehicle_config_dir, '/SubsystemControllerParams.yaml'], + }.items() + ), + ] + ) + # subsystem_controller which orchestrates the lifecycle of this subsystem's components subsystem_controller = Node( package='subsystem_controllers', @@ -218,5 +244,6 @@ def generate_launch_description(): declare_vehicle_config_param_file_arg, declare_subsystem_controller_param_file_arg, carma_guidance_container, + plugins_group, subsystem_controller ]) diff --git a/carma/launch/plugins.launch b/carma/launch/plugins.launch index 0d9e8252d8..320407c1e8 100755 --- a/carma/launch/plugins.launch +++ b/carma/launch/plugins.launch @@ -1,6 +1,6 @@ - - - - - - diff --git a/inlanecruising_plugin/launch/inlanecruising_plugin.launch.py b/inlanecruising_plugin/launch/inlanecruising_plugin.launch.py new file mode 100644 index 0000000000..ed143185b0 --- /dev/null +++ b/inlanecruising_plugin/launch/inlanecruising_plugin.launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA inlanecruising_plugin node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='DEBUG') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='inlanecruising_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='inlanecruising_plugin', + plugin='inlanecruising_plugin::InLaneCruisingPluginNode', + name='inlanecruising_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/inlanecruising_plugin/package.xml b/inlanecruising_plugin/package.xml index 1667fda8a6..3f3fe1c83d 100644 --- a/inlanecruising_plugin/package.xml +++ b/inlanecruising_plugin/package.xml @@ -1,7 +1,7 @@ - - - - - - - - - - - - - - diff --git a/inlanecruising_plugin/test/rostest_ilc.cpp b/inlanecruising_plugin/test/rostest_ilc.cpp index 6c1da8b1f1..f352d1f1b1 100644 --- a/inlanecruising_plugin/test/rostest_ilc.cpp +++ b/inlanecruising_plugin/test/rostest_ilc.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -14,60 +14,71 @@ * the License. */ -#include -#include +#include +#include #include -#include -#include "ros/service.h" +#include #include #include +#include +#include "helper.hpp" +#include +#include -#include "ros/ros.h" -#include + +namespace inlanecruising_plugin +{ TEST(InLaneCruisingPluginTest, rostest1) { - ros::CARMANodeHandle nh; - bool flag_trajectory = false; - bool flag_yield = false; - std::string res = ""; + auto yield_node = std::make_shared(rclcpp::NodeOptions()); + + auto service = yield_node->create_service("yield_plugin/plan_trajectory", + std::bind(&Node::callback, yield_node.get(), std_ph::_1, std_ph::_2, std_ph::_3)); + + yield_node->configure(); + yield_node->activate(); + + auto ilc_node = std::make_shared(rclcpp::NodeOptions()); - cav_srvs::PlanTrajectory traj_srv; - traj_srv.request.initial_trajectory_plan.trajectory_id = "ILCReq"; + ilc_node->configure(); + ilc_node->activate(); + + // Add these nodes to an executor to spin them and trigger callbacks + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(yield_node->get_node_base_interface()); + executor.add_node(ilc_node->get_node_base_interface()); + + carma_planning_msgs::srv::PlanTrajectory::Request traj_srv; + traj_srv.initial_trajectory_plan.trajectory_id = "YieldReq"; - ros::ServiceClient plugin1= nh.serviceClient("inlanecruising_plugin/plan_trajectory"); + InLaneCruisingPluginConfig config; + config.enable_object_avoidance = true; + config.default_downsample_ratio = 1; - ROS_INFO_STREAM("ilc service: " << plugin1.getService()); - if(plugin1.waitForExistence(ros::Duration(5.0))) - { - ros::spinOnce(); - ROS_ERROR("waiting"); - if (plugin1.call(traj_srv)) - { - res = traj_srv.response.trajectory_plan.trajectory_id; - ROS_ERROR("ILC Traj Service called"); - flag_trajectory = true; - flag_yield = true; - - } - else - { - ROS_ERROR("ILC Trajectory Service not called"); - res = "error"; - } + auto yield_srv = std::make_shared(traj_srv); + auto yield_resp = ilc_node->yield_client_->async_send_request(yield_srv); + + // Spin executor for 2 seconds + auto end_time = std::chrono::system_clock::now() + std::chrono::seconds(2); + + while(std::chrono::system_clock::now() < end_time){ + executor.spin_once(); } - EXPECT_TRUE(flag_trajectory); - ASSERT_TRUE(flag_yield); - // EXPECT_EQ(res, "ILC2Yield"); -} + auto future_status = yield_resp.wait_for(std::chrono::milliseconds(100)); + if (future_status == std::future_status::ready) + { + RCLCPP_DEBUG_STREAM(ilc_node->get_logger(), "Received Traj from Yield"); + RCLCPP_INFO_STREAM(rclcpp::get_logger(ILC_LOGGER), "ILC Traj Service called"); + } + else + { + throw std::invalid_argument("Unable to Call Yield Plugin"); + } -int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "rostest_ilc"); - ros::NodeHandle nh; - auto res = RUN_ALL_TESTS(); - return res; -} + ASSERT_EQ(yield_resp.get()->trajectory_plan.trajectory_id, "YieldResp"); +} +} \ No newline at end of file diff --git a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp index 3effec479e..ad0d9acab0 100644 --- a/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp +++ b/inlanecruising_plugin/test/test_inlanecruising_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -14,12 +14,13 @@ * the License. */ -#include +#include +#include #include -#include -#include +#include +#include #include -#include +#include using namespace inlanecruising_plugin; // Test to ensure Eigen::Isometry2d behaves like tf2::Transform @@ -55,638 +56,67 @@ TEST(InLaneCruisingPluginTest, validate_eigen) ASSERT_NEAR(M_PI_2, P_in_A_rot.smallestAngle(), 0.000000001); } -TEST(InLaneCruisingPluginTest, trajectory_from_points_times_orientations) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - lanelet::BasicPoint2d p1(0.0, 0.0); - lanelet::BasicPoint2d p2(2.0, 0.0); - lanelet::BasicPoint2d p3(4.5, 0.0); - lanelet::BasicPoint2d p4(7.0, 3.0); - - std::vector points = { p1, p2, p3, p4 }; - - std::vector times = { 0, 2, 4, 8 }; - std::vector yaws = { 0.2, 0.5, 0.6, 1.0 }; - ros::Time startTime(1.0); - std::vector traj_points = - plugin.trajectory_from_points_times_orientations(points, times, yaws, startTime); - - ASSERT_EQ(4, traj_points.size()); - ASSERT_NEAR(1.0, traj_points[0].target_time.toSec(), 0.0000001); - ASSERT_NEAR(3.0, traj_points[1].target_time.toSec(), 0.0000001); - ASSERT_NEAR(5.0, traj_points[2].target_time.toSec(), 0.0000001); - ASSERT_NEAR(9.0, traj_points[3].target_time.toSec(), 0.0000001); - - ASSERT_NEAR(0.0, traj_points[0].x, 0.0000001); - ASSERT_NEAR(2.0, traj_points[1].x, 0.0000001); - ASSERT_NEAR(4.5, traj_points[2].x, 0.0000001); - ASSERT_NEAR(7.0, traj_points[3].x, 0.0000001); - - ASSERT_NEAR(0.0, traj_points[0].y, 0.0000001); - ASSERT_NEAR(0.0, traj_points[1].y, 0.0000001); - ASSERT_NEAR(0.0, traj_points[2].y, 0.0000001); - ASSERT_NEAR(3.0, traj_points[3].y, 0.0000001); - - ASSERT_NEAR(0.2, traj_points[0].yaw, 0.0000001); - ASSERT_NEAR(0.5, traj_points[1].yaw, 0.0000001); - ASSERT_NEAR(0.6, traj_points[2].yaw, 0.0000001); - ASSERT_NEAR(1.0, traj_points[3].yaw, 0.0000001); - - std::string controller_plugin = "default"; - ASSERT_EQ(0, traj_points[0].controller_plugin_name.compare(controller_plugin)); - ASSERT_EQ(0, traj_points[1].controller_plugin_name.compare(controller_plugin)); - ASSERT_EQ(0, traj_points[2].controller_plugin_name.compare(controller_plugin)); - ASSERT_EQ(0, traj_points[3].controller_plugin_name.compare(controller_plugin)); - - std::string expected_plugin_name = "inlanecruising_plugin"; - ASSERT_EQ(0, traj_points[0].planner_plugin_name.compare(expected_plugin_name)); - ASSERT_EQ(0, traj_points[1].planner_plugin_name.compare(expected_plugin_name)); - ASSERT_EQ(0, traj_points[2].planner_plugin_name.compare(expected_plugin_name)); - ASSERT_EQ(0, traj_points[3].planner_plugin_name.compare(expected_plugin_name)); -} - -TEST(InLaneCruisingPluginTest, constrain_to_time_boundary) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 0); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(6, 0); - points.push_back(p); - p.point = lanelet::BasicPoint2d(7, 0); - points.push_back(p); - - std::vector time_bound_points = plugin.constrain_to_time_boundary(points, 5.0); - - ASSERT_EQ(6, time_bound_points.size()); - ASSERT_NEAR(0.0, time_bound_points[0].point.x(), 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[1].point.x(), 0.0000001); - ASSERT_NEAR(2.0, time_bound_points[2].point.x(), 0.0000001); - ASSERT_NEAR(3.0, time_bound_points[3].point.x(), 0.0000001); - ASSERT_NEAR(4.0, time_bound_points[4].point.x(), 0.0000001); - ASSERT_NEAR(5.0, time_bound_points[5].point.x(), 0.0000001); - - ASSERT_NEAR(0.0, time_bound_points[0].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[1].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[2].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[3].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[4].point.y(), 0.0000001); - ASSERT_NEAR(0.0, time_bound_points[5].point.y(), 0.0000001); - - ASSERT_NEAR(1.0, time_bound_points[0].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[1].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[2].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[3].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[4].speed, 0.0000001); - ASSERT_NEAR(1.0, time_bound_points[5].speed, 0.0000001); -} - -TEST(InLaneCruisingPluginTest, get_nearest_index_by_downtrack_test) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - std::vector points; - std::vector basic_points; - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 0); - p.speed = 1.0; - points.push_back(p); - basic_points.push_back(p.point); - p.point = lanelet::BasicPoint2d(1, 1); - points.push_back(p); - basic_points.push_back(p.point); - p.point = lanelet::BasicPoint2d(2, 2); - points.push_back(p); - basic_points.push_back(p.point); - p.point = lanelet::BasicPoint2d(3, 3); - points.push_back(p); - basic_points.push_back(p.point); - p.point = lanelet::BasicPoint2d(4, 4); - points.push_back(p); - basic_points.push_back(p.point); - p.point = lanelet::BasicPoint2d(5, 5); - points.push_back(p); - basic_points.push_back(p.point); - p.point = lanelet::BasicPoint2d(6, 6); - points.push_back(p); - basic_points.push_back(p.point); - p.point = lanelet::BasicPoint2d(7, 7); - points.push_back(p); - basic_points.push_back(p.point); - - cav_msgs::VehicleState state; - state.x_pos_global = 3.3; - state.y_pos_global = 3.3; - - ASSERT_EQ(3, basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(points, wm, state)); - ASSERT_EQ(3, basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(basic_points, wm, state)); -} - -TEST(InLaneCruisingPluginTest, get_nearest_basic_point_index) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 0); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 1); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 2); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 3); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 4); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 5); - points.push_back(p); - p.point = lanelet::BasicPoint2d(6, 6); - points.push_back(p); - p.point = lanelet::BasicPoint2d(7, 7); - points.push_back(p); - - cav_msgs::VehicleState state; - state.x_pos_global = 3.3; - state.y_pos_global = 3.3; - - //ASSERT_EQ(3, basic_autonomy::waypoint_generation::get_nearest_index_by_downtrack(points, wm, state)); -} - -TEST(InLaneCruisingPluginTest, split_point_speed_pairs) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 1); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 2); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 3); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 4); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 5); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 6); - points.push_back(p); - - std::vector basic_points; - std::vector speeds; - - basic_autonomy::waypoint_generation::split_point_speed_pairs(points, &basic_points, &speeds); - - ASSERT_EQ(points.size(), basic_points.size()); - ASSERT_NEAR(0.0, basic_points[0].x(), 0.0000001); - ASSERT_NEAR(1.0, basic_points[1].x(), 0.0000001); - ASSERT_NEAR(2.0, basic_points[2].x(), 0.0000001); - ASSERT_NEAR(3.0, basic_points[3].x(), 0.0000001); - ASSERT_NEAR(4.0, basic_points[4].x(), 0.0000001); - ASSERT_NEAR(5.0, basic_points[5].x(), 0.0000001); - - ASSERT_NEAR(1.0, basic_points[0].y(), 0.0000001); - ASSERT_NEAR(2.0, basic_points[1].y(), 0.0000001); - ASSERT_NEAR(3.0, basic_points[2].y(), 0.0000001); - ASSERT_NEAR(4.0, basic_points[3].y(), 0.0000001); - ASSERT_NEAR(5.0, basic_points[4].y(), 0.0000001); - ASSERT_NEAR(6.0, basic_points[5].y(), 0.0000001); - - ASSERT_NEAR(1.0, speeds[0], 0.0000001); - ASSERT_NEAR(1.0, speeds[1], 0.0000001); - ASSERT_NEAR(1.0, speeds[2], 0.0000001); - ASSERT_NEAR(1.0, speeds[3], 0.0000001); - ASSERT_NEAR(1.0, speeds[4], 0.0000001); - ASSERT_NEAR(1.0, speeds[5], 0.0000001); -} - -TEST(InLaneCruisingPluginTest, compute_fit) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - /////////////////////// - // Check straight line - /////////////////////// - std::vector points; - auto p = lanelet::BasicPoint2d(20, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(21, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(22, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(23, 30); - points.push_back(p); - - std::unique_ptr fit_curve = plugin.compute_fit(points); - std::vector spline_points; - // Following logic is written for BSpline library. Switch with appropriate call of the new library if different. - double parameter = 0.0; - - for(int i=0; i< points.size(); i++){ - auto values = (*fit_curve)(parameter); - - // Uncomment to print and check if this generated map matches with the original one above - // ROS_INFO_STREAM("BSpline point: x: " << values.x() << "y: " << values.y()); - spline_points.push_back({values.x(),values.y()}); - parameter += 1.0/(points.size()*1.0); - } - - - ASSERT_EQ(spline_points.size(), points.size()); - int error_count = 0; - - tf::Vector3 original_vector_1(points[1].x() - points[0].x(), - points[1].y() - points[0].y(), 0); - original_vector_1.setZ(0); - tf::Vector3 spline_vector_1(spline_points[1].x() - spline_points[0].x(), - spline_points[1].y() - spline_points[0].y(), 0); - spline_vector_1.setZ(0); - tf::Vector3 original_vector_2(points[2].x() - points[1].x(), - points[2].y() - points[1].y(), 0); - original_vector_2.setZ(0); - tf::Vector3 spline_vector_2(spline_points[2].x() - spline_points[1].x(), - spline_points[2].y() - spline_points[1].y(), 0); - spline_vector_2.setZ(0); - double angle_in_rad_1 = std::fabs(tf::tfAngle(original_vector_1, spline_vector_1)); - double angle_in_rad_2 = std::fabs(tf::tfAngle(original_vector_2, spline_vector_2)); - - ASSERT_NEAR(angle_in_rad_1, 0.0, 0.0001); - ASSERT_NEAR(angle_in_rad_2, 0.0, 0.0001); - - /////////////////////// - // S curve - /////////////////////// - points = {}; - lanelet::BasicPoint2d po1(3,4); - points.push_back( po1); - lanelet::BasicPoint2d po2(5,4); - points.push_back( po2); - lanelet::BasicPoint2d po3(8,9); - points.push_back( po3); - lanelet::BasicPoint2d po4(8,23); - points.push_back( po4); - lanelet::BasicPoint2d po5(3.5,25); - points.push_back( po5); - lanelet::BasicPoint2d po6(3,25); - points.push_back( po6); - lanelet::BasicPoint2d po7(2.5,26); - points.push_back( po7); - lanelet::BasicPoint2d po8(2.25,27); - points.push_back( po8); - lanelet::BasicPoint2d po9(2.0,28); - points.push_back( po9); - lanelet::BasicPoint2d po10(1.5,30); - points.push_back(po10); - lanelet::BasicPoint2d po11(1.0,32); - points.push_back(po11); - lanelet::BasicPoint2d po12(1.25,34); - points.push_back(po12); - lanelet::BasicPoint2d po13(2.0,35); - points.push_back(po13); - lanelet::BasicPoint2d po14(4.0,35); - points.push_back(po14); - lanelet::BasicPoint2d po15(5.0,35.5); - points.push_back(po15); - lanelet::BasicPoint2d po16(6.0,36); - points.push_back(po16); - lanelet::BasicPoint2d po17(7.0,50); - points.push_back(po17); - lanelet::BasicPoint2d po18(6.5,48); - points.push_back(po18); - lanelet::BasicPoint2d po19(4.0,43); - points.push_back(po19); - - // As different libraries may fit S curves differently, we are only checking if we can get any fit here. - ASSERT_NO_THROW(plugin.compute_fit(points)); - - std::unique_ptr fit_s_curve = plugin.compute_fit(points); - - ASSERT_TRUE(!!fit_s_curve); - -} - -TEST(InLaneCruisingPluginTest, optimize_speed) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector downtracks, curv_speeds; - downtracks.push_back(0); - downtracks.push_back(2); - downtracks.push_back(4); - downtracks.push_back(6); - downtracks.push_back(8); - downtracks.push_back(10); - downtracks.push_back(12); - downtracks.push_back(14); - downtracks.push_back(16); - - config.max_accel = 2.0; - double max_accel = config.max_accel; - - ASSERT_THROW(plugin.optimize_speed(downtracks, curv_speeds, max_accel), std::invalid_argument); - - curv_speeds.push_back(1); - curv_speeds.push_back(3); - curv_speeds.push_back(4); - curv_speeds.push_back(4); - curv_speeds.push_back(1); - curv_speeds.push_back(0); - curv_speeds.push_back(3); - curv_speeds.push_back(3); - curv_speeds.push_back(6); - - ASSERT_THROW(plugin.optimize_speed(downtracks, curv_speeds, -10), std::invalid_argument); - - std::vector expected_results; - expected_results.push_back(1); - expected_results.push_back(3); - expected_results.push_back(4); - expected_results.push_back(3); - expected_results.push_back(1); - expected_results.push_back(0); - expected_results.push_back(2.82843); - expected_results.push_back(3); - expected_results.push_back(4.12311); - auto test_results = plugin.optimize_speed(downtracks, curv_speeds, max_accel); - - ASSERT_NEAR(expected_results[0], test_results[0], 0.001); - ASSERT_NEAR(expected_results[1], test_results[1], 0.001); - ASSERT_NEAR(expected_results[2], test_results[2], 0.001); - ASSERT_NEAR(expected_results[3], test_results[3], 0.001); - ASSERT_NEAR(expected_results[4], test_results[4], 0.001); - ASSERT_NEAR(expected_results[5], test_results[5], 0.001); - ASSERT_NEAR(expected_results[6], test_results[6], 0.001); - ASSERT_NEAR(expected_results[7], test_results[7], 0.001); - ASSERT_NEAR(expected_results[8], test_results[8], 0.001); - - // Check if the first speed is same - curv_speeds = {}; - curv_speeds.push_back(4); - curv_speeds.push_back(1); - curv_speeds.push_back(3); - curv_speeds.push_back(4); - curv_speeds.push_back(1); - curv_speeds.push_back(0); - curv_speeds.push_back(3); - curv_speeds.push_back(3); - curv_speeds.push_back(6); - - expected_results = {}; - expected_results.push_back(4); - expected_results.push_back(2.82847); - expected_results.push_back(3); - expected_results.push_back(3); - expected_results.push_back(1); - expected_results.push_back(0); - expected_results.push_back(2.82843); - expected_results.push_back(3); - expected_results.push_back(4.12311); - - test_results = plugin.optimize_speed(downtracks, curv_speeds, max_accel); - - ASSERT_NEAR(expected_results[0], test_results[0], 0.001); - ASSERT_NEAR(expected_results[1], test_results[1], 0.001); - ASSERT_NEAR(expected_results[2], test_results[2], 0.001); - ASSERT_NEAR(expected_results[3], test_results[3], 0.001); - ASSERT_NEAR(expected_results[4], test_results[4], 0.001); - ASSERT_NEAR(expected_results[5], test_results[5], 0.001); - ASSERT_NEAR(expected_results[6], test_results[6], 0.001); - ASSERT_NEAR(expected_results[7], test_results[7], 0.001); - ASSERT_NEAR(expected_results[8], test_results[8], 0.001); - -} - -TEST(InLaneCruisingPluginTest, compute_curvature_at) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - /////////////////////// - // Check straight line - /////////////////////// - std::vector points; - auto p = lanelet::BasicPoint2d(20, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(21, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(22, 30); - points.push_back(p); - p = lanelet::BasicPoint2d(23, 30); - points.push_back(p); - std::unique_ptr fit_curve = plugin.compute_fit(points); - - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 0.0), 0, 0.001); // check start - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 1.0), 0, 0.001); // check end - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 0.23), 0, 0.001); // check random 1 - ASSERT_NEAR(plugin.compute_curvature_at((*fit_curve), 0.97), 0, 0.001); // check random 2 - - /////////////////////// - // Circle (0,0 centered, R radius) - /////////////////////// - points = {}; - std::vector x,y; - double x_ = 0.0; - double radius = 20; - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(-sqrt(pow(radius,2) - pow(x_,2))); //y- - x_ += radius/(double)10; - } - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(sqrt(pow(radius,2) - pow(x_,2))); //y+ - x_ -= radius/(double)10; - } - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(sqrt(pow(radius,2) - pow(x_,2))); //y+ - x_ -= radius/(double)10; - } - for (int i = 0; i < 10; i++) - { - x.push_back(x_); - y.push_back(-sqrt(pow(radius,2) - pow(x_,2))); //y- - x_ += radius/(double)10; - } - y.push_back(-sqrt(pow(radius,2) - pow(x_,2))); // to close the loop with redundant first point - - for (auto i = 0; i < y.size(); i ++) - { - points.push_back({x[i],y[i]}); - } - - std::unique_ptr fit_circle = plugin.compute_fit(points); - double param = 0.0; - for (int i = 0 ; i < 40; i ++) - { - auto pt = (*fit_circle)(param); - param += 1.0/40.0; - } - auto pt = (*fit_circle)(param); - - double circle_param = 0.0; - for ( auto i= 0; i < 50; i++) - { - circle_param += 0.02; - } - - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.0), 1.0/radius, 0.005); // check start curvature 1/r - // check curvature is consistent - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.42), plugin.compute_curvature_at((*fit_circle), 0.85), 0.005); - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.0), plugin.compute_curvature_at((*fit_circle), 1.0), 0.005); - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.23), plugin.compute_curvature_at((*fit_circle), 0.99), 0.005); - ASSERT_NEAR(plugin.compute_curvature_at((*fit_circle), 0.12), plugin.compute_curvature_at((*fit_circle), 0.76), 0.005); -} - -TEST(InLaneCruisingPluginTest, attach_back_points) -{ - InLaneCruisingPluginConfig config; - config.default_downsample_ratio = 1; - std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); - - std::vector points; - std::vector future_points; - - PointSpeedPair p; - p.point = lanelet::BasicPoint2d(0, 1); - p.speed = 1.0; - points.push_back(p); - p.point = lanelet::BasicPoint2d(1, 2); - points.push_back(p); - p.point = lanelet::BasicPoint2d(2, 3); - points.push_back(p); - p.point = lanelet::BasicPoint2d(3, 4); - future_points.push_back(p); - points.push_back(p); - p.point = lanelet::BasicPoint2d(4, 5); - future_points.push_back(p); - points.push_back(p); - p.point = lanelet::BasicPoint2d(5, 6); - future_points.push_back(p); - points.push_back(p); - - int nearest_pt_index = 2; - - auto result = plugin.attach_past_points(points, future_points, nearest_pt_index, 1.5); - - ASSERT_EQ(points.size() -1, result.size()); - ASSERT_NEAR(1.0, result[0].point.x(), 0.0000001); - ASSERT_NEAR(2.0, result[1].point.x(), 0.0000001); - ASSERT_NEAR(3.0, result[2].point.x(), 0.0000001); - ASSERT_NEAR(4.0, result[3].point.x(), 0.0000001); - ASSERT_NEAR(5.0, result[4].point.x(), 0.0000001); - - ASSERT_NEAR(2.0, result[0].point.y(), 0.0000001); - ASSERT_NEAR(3.0, result[1].point.y(), 0.0000001); - ASSERT_NEAR(4.0, result[2].point.y(), 0.0000001); - ASSERT_NEAR(5.0, result[3].point.y(), 0.0000001); - ASSERT_NEAR(6.0, result[4].point.y(), 0.0000001); - -} - TEST(InLaneCruisingPluginTest, test_verify_yield) { InLaneCruisingPluginConfig config; config.enable_object_avoidance = true; std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); + auto node = std::make_shared(rclcpp::NodeOptions()); - std::vector trajectory_points; + InLaneCruisingPlugin plugin(node, wm, config, [&](auto msg) {}); - ros::Time startTime(ros::Time::now()); + std::vector trajectory_points; - cav_msgs::TrajectoryPlanPoint point_2; - point_2.x = 5.0; - point_2.y = 0.0; - point_2.target_time = startTime + ros::Duration(1); - point_2.lane_id = "1"; - trajectory_points.push_back(point_2); + rclcpp::Time startTime = node->now(); - cav_msgs::TrajectoryPlanPoint point_3; - point_3.x = 10.0; - point_3.y = 0.0; - point_3.target_time = startTime + ros::Duration(2); - point_3.lane_id = "1"; - trajectory_points.push_back(point_3); + carma_planning_msgs::msg::TrajectoryPlanPoint point_2; + point_2.x = 5.0; + point_2.y = 0.0; + point_2.target_time = startTime + rclcpp::Duration(1, 0); + point_2.lane_id = "1"; + trajectory_points.push_back(point_2); + carma_planning_msgs::msg::TrajectoryPlanPoint point_3; + point_3.x = 10.0; + point_3.y = 0.0; + point_3.target_time = startTime + rclcpp::Duration(2, 0); + point_3.lane_id = "1"; + trajectory_points.push_back(point_3); - cav_msgs::TrajectoryPlan tp; - tp.trajectory_points = trajectory_points; - bool res = plugin.validate_yield_plan(tp); - ASSERT_TRUE(plugin.validate_yield_plan(tp)); + carma_planning_msgs::msg::TrajectoryPlan tp; + tp.trajectory_points = trajectory_points; - cav_msgs::TrajectoryPlan tp2; + bool res = plugin.validate_yield_plan(tp); + ASSERT_TRUE(plugin.validate_yield_plan(tp)); - cav_msgs::TrajectoryPlanPoint point_4; - point_4.x = 5.0; - point_4.y = 0.0; - point_4.target_time = startTime + ros::Duration(1); - point_4.lane_id = "1"; - tp2.trajectory_points.push_back(point_4); - - ASSERT_FALSE(plugin.validate_yield_plan(tp2)); + carma_planning_msgs::msg::TrajectoryPlan tp2; + + carma_planning_msgs::msg::TrajectoryPlanPoint point_4; + point_4.x = 5.0; + point_4.y = 0.0; + point_4.target_time = startTime + rclcpp::Duration(1, 0); + point_4.lane_id = "1"; + tp2.trajectory_points.push_back(point_4); + + ASSERT_FALSE(plugin.validate_yield_plan(tp2)); - cav_msgs::TrajectoryPlan tp3; + carma_planning_msgs::msg::TrajectoryPlan tp3; - cav_msgs::TrajectoryPlanPoint point_5; - point_5.x = 5.0; - point_5.y = 0.0; - point_5.target_time = startTime; - point_5.lane_id = "1"; - tp3.trajectory_points.push_back(point_5); + carma_planning_msgs::msg::TrajectoryPlanPoint point_5; + point_5.x = 5.0; + point_5.y = 0.0; + point_5.target_time = startTime; + point_5.lane_id = "1"; + tp3.trajectory_points.push_back(point_5); - cav_msgs::TrajectoryPlanPoint point_6; - point_6.x = 10.0; - point_6.y = 0.0; - point_6.target_time = startTime + ros::Duration(1); - point_6.lane_id = "1"; - tp3.trajectory_points.push_back(point_6); + carma_planning_msgs::msg::TrajectoryPlanPoint point_6; + point_6.x = 10.0; + point_6.y = 0.0; + point_6.target_time = startTime + rclcpp::Duration(1, 0); + point_6.lane_id = "1"; + tp3.trajectory_points.push_back(point_6); - ASSERT_FALSE(plugin.validate_yield_plan(tp2)); + ASSERT_FALSE(plugin.validate_yield_plan(tp2)); } diff --git a/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp b/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp index fef1e748e4..879a462373 100644 --- a/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp +++ b/inlanecruising_plugin/test/test_inlanecruising_plugin_planning.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -14,10 +14,11 @@ * the License. */ -#include -#include +#include +#include +#include #include -#include +#include #include #include #include @@ -27,32 +28,27 @@ #include #include #include -#include -#include -#include +#include +#include #include -#include -#include - #include #include -#include +#include #include #include #include #include #include #include -#include -#include +#include +#include #include -#include #include #include - #include #include #include +#include typedef Eigen::Spline Spline2d; @@ -65,9 +61,11 @@ namespace inlanecruising_plugin TEST(InLaneCruisingPluginTest, testPlanningCallback) { InLaneCruisingPluginConfig config; + config.enable_object_avoidance = false; config.default_downsample_ratio = 1; std::shared_ptr wm = std::make_shared(); - InLaneCruisingPlugin plugin(wm, config, [&](auto msg) {}); + auto node = std::make_shared(rclcpp::NodeOptions()); + InLaneCruisingPlugin plugin(node, wm, config, [&](auto msg) {}); auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); @@ -91,37 +89,37 @@ TEST(InLaneCruisingPluginTest, testPlanningCallback) carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - cav_srvs::PlanTrajectoryRequest req; + carma_planning_msgs::srv::PlanTrajectory::Request req; req.vehicle_state.x_pos_global = 1.5; req.vehicle_state.y_pos_global = 5; req.vehicle_state.orientation = 0; req.vehicle_state.longitudinal_vel = 0.0; - cav_msgs::Maneuver maneuver; - maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver.lane_following_maneuver.lane_ids = {"1200"}; maneuver.lane_following_maneuver.start_dist = 5.0; - maneuver.lane_following_maneuver.start_time = ros::Time(0.0); + maneuver.lane_following_maneuver.start_time = rclcpp::Time(0.0); maneuver.lane_following_maneuver.start_speed = 0.0; maneuver.lane_following_maneuver.end_dist = 14.98835712; maneuver.lane_following_maneuver.end_speed = 6.7056; - maneuver.lane_following_maneuver.end_time = ros::Time(4.4704); + maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704e9); - cav_msgs::Maneuver maneuver2; - maneuver2.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver maneuver2; + maneuver2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver2.lane_following_maneuver.lane_ids = {"1200"}; maneuver2.lane_following_maneuver.start_dist = 14.98835712; maneuver2.lane_following_maneuver.start_speed = 6.7056; - maneuver2.lane_following_maneuver.start_time = ros::Time(4.4704); + maneuver2.lane_following_maneuver.start_time = rclcpp::Time(4.4704e9); maneuver2.lane_following_maneuver.end_dist = 14.98835712 + 50.0; maneuver2.lane_following_maneuver.end_speed = 6.7056; - maneuver2.lane_following_maneuver.end_time = ros::Time(4.4704 + 7.45645430685); + maneuver2.lane_following_maneuver.end_time = rclcpp::Time((4.4704 + 7.45645430685)*1e9); // Create a third maneuver of a different type to test the final element in resp.related_maneuvers - cav_msgs::Maneuver maneuver3; - maneuver3.type = cav_msgs::Maneuver::LANE_CHANGE; + carma_planning_msgs::msg::Maneuver maneuver3; + maneuver3.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE; req.maneuver_plan.maneuvers.push_back(maneuver); req.maneuver_plan.maneuvers.push_back(maneuver2); @@ -129,11 +127,14 @@ TEST(InLaneCruisingPluginTest, testPlanningCallback) req.maneuver_index_to_plan = 0; - cav_srvs::PlanTrajectoryResponse resp; + carma_planning_msgs::srv::PlanTrajectory::Response resp; + + auto req_ptr = std::make_shared(req); + auto resp_ptr = std::make_shared(resp); - plugin.plan_trajectory_cb(req, resp); + plugin.plan_trajectory_callback(req_ptr, resp_ptr); - EXPECT_EQ(1, resp.related_maneuvers.back()); + EXPECT_EQ(1, resp_ptr->related_maneuvers.back()); } @@ -165,11 +166,13 @@ TEST(WaypointGeneratorTest, DISABLED_test_full_generation) lanelet::MapConformer::ensureCompliance(map, 80_mph); InLaneCruisingPluginConfig config; + config.enable_object_avoidance = false; config.lateral_accel_limit = 1.5; std::shared_ptr wm = std::make_shared(); wm->setMap(map); + auto node = std::make_shared(rclcpp::NodeOptions()); - InLaneCruisingPlugin inlc(wm, config, [&](auto msg) {}); + InLaneCruisingPlugin inlc(node, wm, config, [&](auto msg) {}); auto routing_graph = wm->getMapRoutingGraph(); @@ -179,47 +182,48 @@ TEST(WaypointGeneratorTest, DISABLED_test_full_generation) carma_wm::test::setRouteByIds(route_ids, wm); auto p = wm->getMap()->laneletLayer.get(130).centerline()[3]; - ROS_WARN_STREAM("Start Point: " << p.x() << ", " << p.y()); + RCLCPP_WARN_STREAM(rclcpp::get_logger(ILC_LOGGER), "Start Point: " << p.x() << ", " << p.y()); // -159.666, 521.683 - cav_srvs::PlanTrajectoryRequest req; + carma_planning_msgs::srv::PlanTrajectory::Request req; req.vehicle_state.x_pos_global = -107; req.vehicle_state.y_pos_global = 311.904; req.vehicle_state.orientation = -2.7570977; req.vehicle_state.longitudinal_vel = 0.0; - cav_msgs::Maneuver maneuver; - maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver.lane_following_maneuver.lane_ids = {"110"}; maneuver.lane_following_maneuver.start_dist = 14.98835712 + 45+ 180; - maneuver.lane_following_maneuver.start_time = ros::Time(0.0); + maneuver.lane_following_maneuver.start_time = rclcpp::Time(0.0); maneuver.lane_following_maneuver.start_speed = 0.0; maneuver.lane_following_maneuver.end_dist = 14.98835712 + 50.0 + 45 + 200; maneuver.lane_following_maneuver.end_speed = 6.7056; - maneuver.lane_following_maneuver.end_time = ros::Time(8); + maneuver.lane_following_maneuver.end_time = rclcpp::Time(8e9); - cav_msgs::Maneuver maneuver2; - maneuver2.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver maneuver2; + maneuver2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver2.lane_following_maneuver.lane_ids = {"110"}; maneuver2.lane_following_maneuver.start_dist = 14.98835712 + 45+ 202; maneuver2.lane_following_maneuver.start_speed = 6.7056; - maneuver2.lane_following_maneuver.start_time = ros::Time(4.4704); + maneuver2.lane_following_maneuver.start_time = rclcpp::Time(4.4704e9); maneuver2.lane_following_maneuver.end_dist = 14.98835712 + 50.0 + 45 + 250; maneuver2.lane_following_maneuver.end_speed = 6.7056; - maneuver2.lane_following_maneuver.end_time = ros::Time(4.4704 + 7.45645430685 + 37.31); + maneuver2.lane_following_maneuver.end_time = rclcpp::Time((4.4704 + 7.45645430685 + 37.31)*1e9); req.maneuver_plan.maneuvers.push_back(maneuver); req.maneuver_plan.maneuvers.push_back(maneuver2); - cav_srvs::PlanTrajectoryResponse resp; - - inlc.plan_trajectory_cb(req, resp); + carma_planning_msgs::srv::PlanTrajectory::Response resp; + auto req_ptr = std::make_shared(req); + auto resp_ptr = std::make_shared(resp); + inlc.plan_trajectory_callback(req_ptr, resp_ptr); } TEST(WaypointGeneratorTest, DISABLED_test_compute_fit_full_generation) @@ -245,7 +249,9 @@ TEST(WaypointGeneratorTest, DISABLED_test_compute_fit_full_generation) std::shared_ptr wm = std::make_shared(); wm->setMap(map); - InLaneCruisingPlugin inlc(wm, config, [&](auto msg) {}); + auto node = std::make_shared(rclcpp::NodeOptions()); + + InLaneCruisingPlugin inlc(node, wm, config, [&](auto msg) {}); auto routing_graph = wm->getMapRoutingGraph(); @@ -268,7 +274,7 @@ TEST(WaypointGeneratorTest, DISABLED_test_compute_fit_full_generation) { downsampled_points.push_back(route_geometry[i]); // Uncomment to print and check if this original map matches with the generated one below - // ROS_INFO_STREAM("Original point: x: " << route_geometry[i].x() << "y: " << route_geometry[i].y()); + // RCLCPP_INFO_STREAM(rclcpp::get_logger(ILC_LOGGER), "Original point: x: " << route_geometry[i].x() << "y: " << route_geometry[i].y()); } std::unique_ptr fit_curve = basic_autonomy:: waypoint_generation::compute_fit(downsampled_points); @@ -280,7 +286,7 @@ TEST(WaypointGeneratorTest, DISABLED_test_compute_fit_full_generation) for(int i=0; i< downsampled_points.size(); i++){ lanelet::BasicPoint2d pt = (*fit_curve)(parameter); // Uncomment to print and check if this generated map matches with the original one above - // ROS_INFO_STREAM("BSpline point: x: " << values.x() << "y: " << values.y()); + // RCLCPP_INFO_STREAM(rclcpp::get_logger(ILC_LOGGER), "BSpline point: x: " << values.x() << "y: " << values.y()); spline_points.push_back(pt); parameter += 1.0/(downsampled_points.size()*1.0); } @@ -291,33 +297,39 @@ TEST(WaypointGeneratorTest, DISABLED_test_compute_fit_full_generation) for(int i = 1; i < downsampled_points.size(); i ++) { // tag as erroneous if directions of generated points are not within 5 degree of those of original points - tf::Vector3 original_vector(downsampled_points[i].x() - downsampled_points[i-1].x(), + tf2::Vector3 original_vector(downsampled_points[i].x() - downsampled_points[i-1].x(), downsampled_points[i].y() - downsampled_points[i-1].y(), 0); original_vector.setZ(0); - tf::Vector3 spline_vector(spline_points[i].x() - spline_points[i-1].x(), + tf2::Vector3 spline_vector(spline_points[i].x() - spline_points[i-1].x(), spline_points[i].y() - spline_points[i-1].y(), 0); spline_vector.setZ(0); - double angle_in_rad = std::fabs(tf::tfAngle(original_vector, spline_vector)); + double angle_in_rad = std::fabs(tf2::tf2Angle(original_vector, spline_vector)); if (angle_in_rad > 0.08) error_count ++; } // We say it is passing if there is less than 10% error in total number of points - ROS_INFO_STREAM("Total points above 5 degree difference in their direction:" << error_count + RCLCPP_INFO_STREAM(rclcpp::get_logger(ILC_LOGGER), "Total points above 5 degree difference in their direction:" << error_count << ", which is " << (double)error_count/(double)downsampled_points.size()*100 << "% of total"); ASSERT_TRUE((double)error_count/(double)downsampled_points.size() < 0.1); } + }; // namespace inlanecruising_plugin // Run all the tests -int main(int argc, char **argv) +int main(int argc, char ** argv) { - testing::InitGoogleTest(&argc, argv); - ros::Time::init(); - ROSCONSOLE_AUTOINIT; - if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { - ros::console::notifyLoggerLevelsChanged(); - } - return RUN_ALL_TESTS(); -} \ No newline at end of file + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + auto ret = rcutils_logging_set_logger_level("inlanecruising_plugin", RCUTILS_LOG_SEVERITY_DEBUG); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} \ No newline at end of file diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 5a716f77c2..83fc10dfdb 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -19,8 +19,8 @@ - /guidance/trajectory_executor - /guidance/twist_filter - /guidance/twist_gate - - /guidance/route_following_plugin # The minimal set of guidance plugins for system operation are route_following/inlane_cruising/pure_pursuit - - /guidance/inlanecruising_plugin + - /guidance/plugins/route_following_plugin # The minimal set of guidance plugins for system operation are route_following/inlanecruising/pure_pursuit + - /guidance/plugins/inlanecruising_plugin - /guidance/pure_pursuit_wrapper - /guidance/pure_pursuit - /guidance/yield_plugin @@ -38,14 +38,14 @@ required_plugins: - /guidance/plugins/route_following_plugin - /guidance/plugins/pure_pursuit_wrapper - - /guidance/plugins/inlane_cruising_plugin + - /guidance/plugins/inlanecruising_plugin - /guidance/plugins/cooperative_lanechange # List of guidance plugins which are not required but the user wishes to have automatically activated # so that the user doesn't need to manually activate them via the UI on each launch (though they still can) # this list should have zero intersection with the required_plugins auto_activated_plugins: - - /guidance/plugins/lic_strategic_plugin + - /guidance/plugins/lci_strategic_plugin - /guidance/plugins/intersection_transit_maneuvering - /guidance/plugins/light_controlled_intersection_tactical_plugin - /guidance/plugins/stop_and_wait_plugin @@ -54,4 +54,8 @@ - /guidance/plugins/platoon_control_ihp - /guidance/plugins/platoon_strategic_ihp - /guidance/plugins/platooning_tactical_plugin - - /guidance/plugins/yield_plugin \ No newline at end of file + - /guidance/plugins/yield_plugin + + # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. + ros2_initial_plugins: + - /guidance/plugins/inlanecruising_plugin \ No newline at end of file diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h index c39bb7d84f..ce70b72660 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/entry.h @@ -43,8 +43,8 @@ namespace subsystem_controllers /** * \brief All fields constructor */ - Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability, bool user_requested_activation) - : available_(available), active_(active), name_(name), type_(type), capability_(capability), user_requested_activation_(user_requested_activation) {} + Entry(bool available, bool active, const std::string& name, uint8_t type, const std::string& capability, bool user_requested_activation, bool is_ros1) + : available_(available), active_(active), name_(name), type_(type), capability_(capability), user_requested_activation_(user_requested_activation), is_ros1_(is_ros1) {} Entry() = default; diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp index 324c2ee32d..6b806b8d12 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/guidance_controller_config.hpp @@ -37,6 +37,9 @@ namespace subsystem_controllers // this list should have zero intersection with the required_plugins std::vector auto_activated_plugins; + //! List of guidance plugins that are ROS2. If it is not in the list, it is assumed to be ROS1 and not managed + std::vector ros2_initial_plugins; + // Stream operator for this config friend std::ostream &operator<<(std::ostream &output, const GuidanceControllerConfig &c) { @@ -52,7 +55,12 @@ namespace subsystem_controllers for (auto node : c.auto_activated_plugins) output << node << " "; - output << "] " << std::endl + output << "] " << std::endl << "ros2_initial_plugins: [ "; + + for (auto node : c.ros2_initial_plugins) + output << node << " "; + + output << "] " << std::endl << "}" << std::endl; return output; } diff --git a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h index 3721c45448..49191fb2bc 100644 --- a/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h +++ b/subsystem_controllers/include/subsystem_controllers/guidance_controller/plugin_manager.h @@ -56,6 +56,7 @@ namespace subsystem_controllers * * \param required_plugins The set of plugins which will be treated as required. A failure in these plugins will result in an exception * \param auto_activated_plugins The set of plugins which will be automatically activated at first system activation but not treated specially after that. + * \param ros2_initial_plugins The set of plugins which will be managed as they are ROS2. * \param plugin_lifecycle_mgr A fully initialized lifecycle manager which will be used trigger plugin transitions * \param get_parent_state_func A callback which will allow this object to access the parent process lifecycle state * \param get_service_names_and_types_func A callback which returns a map of service names to service types based on the provided base node name and namespace @@ -64,6 +65,7 @@ namespace subsystem_controllers */ PluginManager(const std::vector& required_plugins, const std::vector& auto_activated_plugins, + const std::vector& ros2_initial_plugins, std::shared_ptr plugin_lifecycle_mgr, GetParentNodeStateFunc get_parent_state_func, ServiceNamesAndTypesFunc get_service_names_and_types_func, @@ -193,6 +195,10 @@ namespace subsystem_controllers // These will only be activated once, if the user later deactivates them then that behavior will be preserved std::unordered_set auto_activated_plugins_; + //! Set of ROS2 plugins that will be managed initially. In other words, newly detected plugins after initial configuration are also managed appropriately + // despite not being in the list. + std::unordered_set ros2_initial_plugins_; + //! Lifecycle Manager which will track the plugin nodes and call their lifecycle services on request std::shared_ptr plugin_lifecycle_mgr_; diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index 4f95f1578d..bb89dbf4fb 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -28,6 +28,9 @@ namespace subsystem_controllers // 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_.required_plugins = declare_parameter>("required_plugins", config_.required_plugins); + config_.auto_activated_plugins = declare_parameter>("auto_activated_plugins", config_.auto_activated_plugins); + config_.ros2_initial_plugins = declare_parameter>("ros2_initial_plugins", config_.ros2_initial_plugins); } cr2::CallbackReturn GuidanceControllerNode::handle_on_configure(const rclcpp_lifecycle::State &prev_state) { @@ -58,6 +61,7 @@ namespace subsystem_controllers // Load required plugins and default enabled plugins get_parameter>("required_plugins", config_.required_plugins); get_parameter>("auto_activated_plugins", config_.auto_activated_plugins); + get_parameter>("ros2_initial_plugins", config_.ros2_initial_plugins); RCLCPP_INFO_STREAM(get_logger(), "Config: " << config_); @@ -67,7 +71,8 @@ namespace subsystem_controllers plugin_manager_ = std::make_shared( config_.required_plugins, - config_.auto_activated_plugins, + config_.auto_activated_plugins, + config_.ros2_initial_plugins, plugin_lifecycle_manager, [this](){ return get_current_state().id(); }, [this](auto node, auto ns){ return get_service_names_and_types_by_node(node, ns); }, diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 3dd8fa0da3..edee75dd54 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -27,34 +27,38 @@ namespace subsystem_controllers PluginManager::PluginManager(const std::vector& required_plugins, const std::vector& auto_activated_plugins, + const std::vector& ros2_initial_plugins, std::shared_ptr plugin_lifecycle_mgr, GetParentNodeStateFunc get_parent_state_func, ServiceNamesAndTypesFunc get_service_names_and_types_func, std::chrono::nanoseconds service_timeout, std::chrono::nanoseconds call_timeout) : required_plugins_(required_plugins.begin(), required_plugins.end()), auto_activated_plugins_(auto_activated_plugins.begin(), auto_activated_plugins.end()), + ros2_initial_plugins_(ros2_initial_plugins.begin(), ros2_initial_plugins.end()), plugin_lifecycle_mgr_(plugin_lifecycle_mgr), get_parent_state_func_(get_parent_state_func), get_service_names_and_types_func_(get_service_names_and_types_func), service_timeout_(service_timeout), call_timeout_(call_timeout) { if (!plugin_lifecycle_mgr) throw std::invalid_argument("Input plugin_lifecycle_mgr to PluginManager constructor cannot be null"); - + // For all required and auto activated plugins add unknown entries but with // user_requested_activation set to true. // This will be used later to determine how to transition the plugin specified by that entry + for (const auto& p : required_plugins_) { - Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + bool is_ros1 = ros2_initial_plugins_.find(p) == ros2_initial_plugins_.end(); + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true, is_ros1); em_.update_entry(e); plugin_lifecycle_mgr_->add_managed_node(p); } for (const auto& p : auto_activated_plugins_) { - Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true); + bool is_ros1 = ros2_initial_plugins_.find(p) == ros2_initial_plugins_.end(); + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true, is_ros1); em_.update_entry(e); plugin_lifecycle_mgr_->add_managed_node(p); } - } bool PluginManager::is_ros2_lifecycle_node(const std::string& node) @@ -232,7 +236,7 @@ namespace subsystem_controllers // If this is not a plugin slated for activation then continue and leave up to user to activate manually later if (!plugin.user_requested_activation_) continue; - + auto result_state = plugin_lifecycle_mgr_->transition_node_to_state(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, plugin.name_, service_timeout_, call_timeout_); if(result_state != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) @@ -421,9 +425,7 @@ namespace subsystem_controllers activated = (result_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } - - - Entry updated_entry(requested_plugin->available_, activated, requested_plugin->name_, requested_plugin->type_, requested_plugin->capability_, true); // Mark as user activated + Entry updated_entry(requested_plugin->available_, activated, requested_plugin->name_, requested_plugin->type_, requested_plugin->capability_, true, requested_plugin->is_ros1_); // Mark as user activated em_.update_entry(updated_entry); res->newstate = activated; @@ -431,18 +433,17 @@ namespace subsystem_controllers void PluginManager::update_plugin_status(carma_planning_msgs::msg::Plugin::UniquePtr msg) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("guidance_controller"), "received status from: " << msg->name); boost::optional requested_plugin = em_.get_entry_by_name(msg->name); if (!requested_plugin) // This is a new plugin so we need to add it { - Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, false); + Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, false, false); //is_ros1 flag is updated appropriately in add_plugin add_plugin(plugin); return; } - Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, requested_plugin->user_requested_activation_); - + Entry plugin(msg->available, msg->activated, msg->name, msg->type, msg->capability, requested_plugin->user_requested_activation_, requested_plugin->is_ros1_); + em_.update_entry(plugin); } diff --git a/subsystem_controllers/test/test_plugin_manager.cpp b/subsystem_controllers/test/test_plugin_manager.cpp index 7a7b1ea1cb..16810fc701 100644 --- a/subsystem_controllers/test/test_plugin_manager.cpp +++ b/subsystem_controllers/test/test_plugin_manager.cpp @@ -127,6 +127,7 @@ namespace subsystem_controllers { std::vector req_plugins = {"plg_1"}; std::vector auto_actv_plugins = {"plg_2"}; + std::vector ros2_plugins = {"plg_2", "plg_1"}; auto mlm = std::make_shared(); @@ -135,6 +136,7 @@ namespace subsystem_controllers PluginManager pm( req_plugins, auto_actv_plugins, + ros2_plugins, mlm, [&curr_state](){ return curr_state; }, [](auto node, auto) -> std::map>> { From ceeee95f0f025fe4964bbb1dd899cdcb57af450a Mon Sep 17 00:00:00 2001 From: JonSmet <34752540+JonSmet@users.noreply.github.com> Date: Fri, 15 Jul 2022 11:48:10 -0400 Subject: [PATCH 033/165] Port the port_drayage_plugin to ROS2 (#1840) * port port_drayage_plugin to ROS2 * Revert default log level for guidance packages * Use operation enum directly * Use OperationId object for stream operator * Fix formatting * Enable PortDrayageWorker updates from parameter update callback --- carma/launch/guidance.launch | 45 +- carma/launch/guidance.launch.py | 23 + port_drayage_plugin/CMakeLists.txt | 146 +-- port_drayage_plugin/README.md | 5 + port_drayage_plugin/config/parameters.yaml | 8 + port_drayage_plugin/config/params.yml | 5 - .../port_drayage_plugin/port_drayage_plugin.h | 159 ---- .../port_drayage_plugin.hpp | 104 +++ .../port_drayage_plugin_config.hpp | 51 ++ ...chine.h => port_drayage_state_machine.hpp} | 45 +- ...ayage_worker.h => port_drayage_worker.hpp} | 229 ++--- .../launch/port_drayage_plugin.launch | 22 - .../launch/port_drayage_plugin_launch.py | 68 ++ port_drayage_plugin/package.xml | 52 +- ...{port_drayage_plugin_main.cpp => main.cpp} | 29 +- .../src/port_drayage_plugin.cpp | 401 +++------ .../src/port_drayage_state_machine.cpp | 54 +- .../src/port_drayage_worker.cpp | 333 ++++--- .../test/test_port_drayage_plugin.cpp | 847 +++++++----------- 19 files changed, 1119 insertions(+), 1507 deletions(-) create mode 100644 port_drayage_plugin/README.md create mode 100644 port_drayage_plugin/config/parameters.yaml delete mode 100644 port_drayage_plugin/config/params.yml delete mode 100644 port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.h create mode 100644 port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.hpp create mode 100644 port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin_config.hpp rename port_drayage_plugin/include/port_drayage_plugin/{port_drayage_state_machine.h => port_drayage_state_machine.hpp} (69%) rename port_drayage_plugin/include/port_drayage_plugin/{port_drayage_worker.h => port_drayage_worker.hpp} (53%) delete mode 100644 port_drayage_plugin/launch/port_drayage_plugin.launch create mode 100644 port_drayage_plugin/launch/port_drayage_plugin_launch.py rename port_drayage_plugin/src/{port_drayage_plugin_main.cpp => main.cpp} (51%) diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 25c1d1be81..671c586863 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -215,14 +215,51 @@ - + + + + + + + + + + + + + + + + + + + + + + + - - + - + + + + + + + + + + + + diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index c9bab1eccc..595f6d895a 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -72,6 +72,9 @@ def generate_launch_description(): plan_delegator_param_file = os.path.join( get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml') + + port_drayage_plugin_param_file = os.path.join( + get_package_share_directory('port_drayage_plugin'), 'config/parameters.yaml') inlanecruising_plugin_file_path = os.path.join( get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml') @@ -207,6 +210,26 @@ def generate_launch_description(): guidance_param_file ] ), + ComposableNode( + package='port_drayage_plugin', + plugin='port_drayage_plugin::PortDrayagePlugin', + name='port_drayage_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('route', env_log_levels) } + ], + remappings = [ + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), + ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), + ("ui_instructions", [ EnvironmentVariable('CARMA_UI_NS', default_value=''), "/ui_instructions" ] ) + ], + parameters=[ + port_drayage_plugin_param_file, + vehicle_characteristics_param_file + ] + ) ] ) diff --git a/port_drayage_plugin/CMakeLists.txt b/port_drayage_plugin/CMakeLists.txt index dc18a7feeb..c8d448b60f 100644 --- a/port_drayage_plugin/CMakeLists.txt +++ b/port_drayage_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ -# Copyright (C) 2020-2021 LEIDOS. +# Copyright (C) 2020-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 @@ -13,136 +13,62 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(port_drayage_plugin) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - carma_utils - cav_msgs - cav_srvs - geometry_msgs - roscpp - std_msgs - carma_wm -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES port_drayage_plugin - CATKIN_DEPENDS carma_utils cav_msgs cav_srvs roscpp std_msgs carma_wm -# DEPENDS system_lib -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -########### -## Build ## -########### +# Name build targets +set(node_exec port_drayage_plugin_node_exec) +set(node_lib port_drayage_plugin_node) -## Specify additional locations of header files -## Your package locations should be listed before other locations +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -add_library(port_drayage_plugin_library - src/port_drayage_plugin.cpp - src/port_drayage_state_machine.cpp - src/port_drayage_worker.cpp +# Build +ament_auto_add_library(${node_lib} SHARED + src/port_drayage_plugin.cpp + src/port_drayage_state_machine.cpp + src/port_drayage_worker.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(port_drayage_plugin_library ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME} src/port_drayage_plugin_main.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -set_target_properties(${PROJECT_NAME} PROPERTIES OUTPUT_NAME port_drayage_plugin PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(port_drayage_plugin_library - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} +ament_auto_add_executable(${node_exec} + src/main.cpp ) -target_link_libraries(${PROJECT_NAME} - port_drayage_plugin_library +# Register component +rclcpp_components_register_nodes(${node_lib} "port_drayage_plugin::PortDrayagePlugin") + +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -############# -## Install ## -############# +# Testing +if(BUILD_TESTING) -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS ${PROJECT_NAME} port_drayage_plugin_library - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable# -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS ${PROJECT_NAME} port_drayage_plugin_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) + ament_add_gtest(test_port_drayage_plugin test/test_port_drayage_plugin.cpp) -## Mark cpp header files for installation -# install(DIRECTORY include -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + ament_target_dependencies(test_port_drayage_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -############# -## Testing ## -############# + target_link_libraries(test_port_drayage_plugin ${node_lib}) -## Add gtest based cpp test target and link libraries -catkin_add_gtest(${PROJECT_NAME}-test test/test_port_drayage_plugin.cpp) -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test - port_drayage_plugin_library - ${catkin_LIBRARIES}) endif() -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/port_drayage_plugin/README.md b/port_drayage_plugin/README.md new file mode 100644 index 0000000000..c3191b204f --- /dev/null +++ b/port_drayage_plugin/README.md @@ -0,0 +1,5 @@ +# port_drayage_plugin + +The Port Drayage Plugin is a ROS node within CARMA Platform that supports freight port drayage operations. It is responsible for handling all port drayage-related communication with V2X-Hub, and for facilitating interactions with other ROS nodes (primarily the route node) within CARMA Platform in order for the vehicle to execute port drayage operations successfully. + +Link to detailed design document on Confluence: [Click Here](https://usdot-carma.atlassian.net/wiki/spaces/CRMFRT/pages/2074771465/Detailed+Design+-+Port+Drayage+Plugin+CARMA+Platform) \ No newline at end of file diff --git a/port_drayage_plugin/config/parameters.yaml b/port_drayage_plugin/config/parameters.yaml new file mode 100644 index 0000000000..fb6a7640ee --- /dev/null +++ b/port_drayage_plugin/config/parameters.yaml @@ -0,0 +1,8 @@ +# String: The ID of the cargo that is being carried at system start-up. Empty if CMV is not carrying cargo. +cargo_id: "" + +# Bool: Flag to activate/deactivate port drayage operations. If false, the port drayage state machine will remain INACTIVE +enable_port_drayage: false + +# Bool: Flag to indicate CMV's first destination; 'true' indicates Staging Area Entrance, 'false' indicates Port Entrance +starting_at_staging_area: true \ No newline at end of file diff --git a/port_drayage_plugin/config/params.yml b/port_drayage_plugin/config/params.yml deleted file mode 100644 index 399d2aa1fd..0000000000 --- a/port_drayage_plugin/config/params.yml +++ /dev/null @@ -1,5 +0,0 @@ -stop_speed_epsilon: 1.0 -cargo_id: "" # Test value (Empty string indicates the vehicle is not carrying cargo); CARMA Streets expects cargo_id to be a string -enable_port_drayage: false # bool: Flag to activate and deactivate port drayage operations. If false, state machine will remain INACTIVE. -starting_at_staging_area: true # bool: Flag indicating CMV's first destination; 'true' indicates Staging Area Entrance; 'false' indicates Port Entrance. -host_id: "HOST_ID" diff --git a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.h b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.h deleted file mode 100644 index 9164ccc76b..0000000000 --- a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.h +++ /dev/null @@ -1,159 +0,0 @@ -#pragma once - -/* - * 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. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace port_drayage_plugin -{ - /** - * Primary Port Drayage Plugin implementation class. Split into this class - * primarily concerned with the handling of ROS message processing and the - * PortDrayageWorker class responsible for handling the core business logic - * of the Port Drayage functionality. - */ - class PortDrayagePlugin - { - private: - std::shared_ptr _nh = nullptr; - std::shared_ptr _pnh = nullptr; - std::shared_ptr _maneuver_plan_subscriber = nullptr; - std::shared_ptr _pose_subscriber = nullptr; - std::shared_ptr _cur_speed_subscriber = nullptr; - std::shared_ptr _inbound_mobility_operation_subscriber = nullptr; - std::shared_ptr _guidance_state_subscriber = nullptr; - std::shared_ptr _route_event_subscriber = nullptr; - std::shared_ptr _georeference_subscriber = nullptr; - std::shared_ptr _outbound_mobility_operations_publisher = nullptr; - std::shared_ptr _ui_instructions_publisher = nullptr; - - // ROS service servers - ros::ServiceServer plan_maneuver_srv_; - - // ROS service clients - ros::ServiceClient _set_active_route_client; - - /** - * \brief Calls the /guidance/set_active_route service client to set an active route - * \param req The service request being used to call the service client - * \return If the service client call was successful and no errors occurred while setting the new active route - */ - bool call_set_active_route_client(cav_srvs::SetActiveRoute req); - - public: - double declaration; - std::shared_ptr curr_pose_ = nullptr; - geometry_msgs::Twist _cur_speed; - - // wm listener pointer and pointer to the actual wm object - std::shared_ptr wml_; - carma_wm::WorldModelConstPtr wm_; - - /** - * \brief Basic constructor for initializing the Port Drayage Plugin - * - * \param nh A shared ptr to a public node handle for this node - * \param pnh A shared ptr to a private node handle for this node - */ - PortDrayagePlugin(std::shared_ptr nh, - std::shared_ptr pnh) : - _nh(nh), - _pnh(pnh) {}; - - /** - * \brief Testing constructor for initializing the Port Drayage Plugin - * - * Intended for use without ROS, so the nh and pnh are left as null - */ - PortDrayagePlugin() : - _nh(nullptr), - _pnh(nullptr) {}; - - /** - * \brief Begin execution of the Port Drayage Plugin functionality - * - * \return The exit code of the application - */ - int run(); - - /** - * \brief Service callback for arbitrator maneuver planning - * \param req Plan maneuver request - * \param resp Plan maneuver response with a list of maneuver plan - * \return If service call successed - */ - bool plan_maneuver_cb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp); - - /** - * \brief compose Maneuver Message to send to tactical plugin. - * \param current_dist Start downtrack distance of the current maneuver - * \param end_dist End downtrack distance of the current maneuver - * \param current_speed Start speed of the current maneuver - * \param target_speed Target speed pf the current maneuver, usually it is the lanelet speed limit - * \param lane_id Lanelet ID of the current maneuver - * \param current_time Start time of the current maneuver - * \return A stop wait maneuver message which is ready to be published - */ - cav_msgs::Maneuver compose_stop_and_wait_maneuver_message(double current_dist, - double end_dist, - double current_speed, - double target_speed, - int lane_id, - ros::Time time, - double time_to_stop); - - /** - * \brief compose Maneuver Message to send to tactical plugin. - * \param current_dist Start downtrack distance of the current maneuver - * \param end_dist End downtrack distance of the current maneuver - * \param current_speed Start speed of the current maneuver - * \param target_speed Target speed pf the current maneuver, usually it is the lanelet speed limit - * \param lane_id Lanelet ID of the current maneuver - * \param current_time Start time of the current maneuver - * \return A stop wait maneuver message which is ready to be published - */ - cav_msgs::Maneuver compose_lane_following_maneuver_message(double current_dist, - double end_dist, - double current_speed, - double target_speed, - int lane_id, - ros::Time current_time); - - - }; - - double estimate_distance_to_stop(double v, double a); - double estimate_time_to_stop(double d, double v); - -} // namespace port_drayage_plugin diff --git a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.hpp b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.hpp new file mode 100644 index 0000000000..62a2050714 --- /dev/null +++ b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin.hpp @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2020-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. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "port_drayage_plugin/port_drayage_plugin_config.hpp" +#include "port_drayage_plugin/port_drayage_worker.hpp" + +namespace port_drayage_plugin +{ + + /** + * Primary Port Drayage Plugin implementation class. Split into this class + * primarily concerned with the handling of ROS message processing and the + * PortDrayageWorker class responsible for handling the core business logic + * of the Port Drayage functionality. + */ + class PortDrayagePlugin : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Subscribers + carma_ros2_utils::SubPtr pose_subscriber_; + carma_ros2_utils::SubPtr inbound_mobility_operation_subscriber_; + carma_ros2_utils::SubPtr guidance_state_subscriber_; + carma_ros2_utils::SubPtr route_event_subscriber_; + carma_ros2_utils::SubPtr georeference_subscriber_; + + // Publishers + carma_ros2_utils::PubPtr outbound_mobility_operations_publisher_; + carma_ros2_utils::PubPtr ui_instructions_publisher_; + + // Service Clients + carma_ros2_utils::ClientPtr set_active_route_client_; + + // Node configuration + Config config_; + + // PortDrayageWorker object + PortDrayageWorker pdw_; + + public: + /** + * \brief PortDrayagePlugin constructor + */ + explicit PortDrayagePlugin(const rclcpp::NodeOptions &); + + /** + * \brief Calls the /guidance/set_active_route service client to set an active route + * \param req The service request being used to call the service client + * \return If the service client call was successful and no errors occurred while setting the new active route + */ + bool callSetActiveRouteClient(std::shared_ptr req); + + /** + * \brief Publishes a Mobility Operation message to the outgoing mobility operation topic + * \param msg The Mobility Operation message to be published + */ + void publishMobilityOperation(const carma_v2x_msgs::msg::MobilityOperation& msg); + + /** + * \brief Publishes a UI Instructions message to the outgoing UI Instructions topic + * \param msg The UI Instructions message to be published + */ + void publishUIInstructions(const carma_msgs::msg::UIInstructions& msg); + + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &prev_state); + }; + +} // port_drayage_plugin diff --git a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin_config.hpp b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin_config.hpp new file mode 100644 index 0000000000..43a27792fa --- /dev/null +++ b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_plugin_config.hpp @@ -0,0 +1,51 @@ +#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. + */ + +#include +#include + +namespace port_drayage_plugin +{ + + /** + * \brief Struct containing the algorithm configuration values for port_drayage_plugin + */ + struct Config + { + std::string cmv_id = ""; // The CMV's static ID, which is its license plate + + std::string cargo_id = ""; // The ID of the cargo that is being carried at system start-up. Empty if CMV is not carrying cargo. + + bool enable_port_drayage = false; // Flag to activate/deactivate port drayage operations. If false, the port drayage state machine will remain INACTIVE + + bool starting_at_staging_area = true; // Flag to indicate CMV's first destination; 'true' indicates Staging Area Entrance, 'false' indicates Port Entrance + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "port_drayage_plugin::Config { " << std::endl + << "cmv_id: " << c.cmv_id << std::endl + << "cargo_id: " << c.cargo_id << std::endl + << "enable_port_drayage: " << c.enable_port_drayage << std::endl + << "starting_at_staging_area: " << c.starting_at_staging_area << std::endl + << "}" << std::endl; + return output; + } + }; + +} // port_drayage_plugin \ No newline at end of file diff --git a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_state_machine.h b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_state_machine.hpp similarity index 69% rename from port_drayage_plugin/include/port_drayage_plugin/port_drayage_state_machine.h rename to port_drayage_plugin/include/port_drayage_plugin/port_drayage_state_machine.hpp index 9cb9f85e6e..db1e4b173f 100644 --- a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_state_machine.h +++ b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_state_machine.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2020-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 @@ -16,6 +16,7 @@ * the License. */ +#include #include namespace port_drayage_plugin @@ -25,10 +26,11 @@ namespace port_drayage_plugin * * INACTIVE = PortDrayagePlugin is not currently operating in an active * capacity - * EN_ROUTE = The vehicle is currently EN_ROUTE to a destination, either - * under the direciton of the PortDrayagePlugin or otherwise. - * AWAITING_DIRECTION = The vehicle is currently stopped under the command - * of the PortDrayagePlugin and is awaiting further guidance from the + * EN_ROUTE_TO_INITIAL_DESTINATION = The vehicle is currently enroute to its initial + * destination, the type of which is based on the configuration parameters for this node + * EN_ROUTE_TO_RECEIVED_DESTINATION = The vehicle is currently enroute to a destination + * that was received from infrastructure + * AWAITING_DIRECTION = The vehicle is currently stopped and is awaiting further guidance from the * port infrastructure to tell it the next destination. */ enum PortDrayageState @@ -69,11 +71,14 @@ namespace port_drayage_plugin class PortDrayageStateMachine { private: - PortDrayageState _state; - std::function _on_system_startup; - std::function _on_received_new_destination; - std::function _on_arrived_at_destination; - std::function _on_drayage_completed; + PortDrayageState state_; + std::function on_system_startup_; + std::function on_received_new_destination_; + std::function on_arrived_at_destination_; + std::function on_drayage_completed_; + + // Logger interface for this object + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; public: /** @@ -81,49 +86,49 @@ namespace port_drayage_plugin * * The system initially starts in INACTIVE state. */ - PortDrayageStateMachine() : - _state(PortDrayageState::INACTIVE) {}; + PortDrayageStateMachine(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) : + logger_(logger), + state_(PortDrayageState::INACTIVE) {}; /** * \Brief Inform the state machine that an event has transpired. * * \param event The event that took place. */ - void process_event(PortDrayageEvent event); + void processEvent(PortDrayageEvent event); /** * \brief Get the current state of the state machine * \return The current state value */ - PortDrayageState get_state() const; + PortDrayageState getState() const; /** * \brief Set the callback to be invoked upon transitioning out of the * inactive state. * \param cb The std::function callback object */ - void set_on_system_startup_callback(const std::function &cb); + void setOnSystemStartupCallback(const std::function &cb); /** * \brief Set the callback to be invoked upon transitioning into the * EN_ROUTE state * \param cb The std::function callback object */ - void set_on_received_new_destination_callback(const std::function &cb); + void setOnReceivedNewDestinationCallback(const std::function &cb); /** * \brief Set the callback to be invoked upon transitioning into the * AWAITING_DESTINATION state * \param cb The std::function callback object */ - void set_on_arrived_at_destination_callback(const std::function &cb); + void setOnArrivedAtDestinationCallback(const std::function &cb); /** * \brief Set the callback to be invoked upon transitioning into the * inactive state. * \param cb The std::function callback object */ - void set_on_drayage_completed_callback(const std::function &cb); + void setOnDrayageCompletedCallback(const std::function &cb); }; -} // namespace port_drayage_plugin - +} // namespace port_drayage_plugin \ No newline at end of file diff --git a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_worker.h b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_worker.hpp similarity index 53% rename from port_drayage_plugin/include/port_drayage_plugin/port_drayage_worker.h rename to port_drayage_plugin/include/port_drayage_plugin/port_drayage_worker.hpp index 0ef27f89d0..5c25fff52d 100644 --- a/port_drayage_plugin/include/port_drayage_plugin/port_drayage_worker.h +++ b/port_drayage_plugin/include/port_drayage_plugin/port_drayage_worker.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2020-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 @@ -16,18 +16,22 @@ * the License. */ -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/string.hpp" +#include "port_drayage_plugin/port_drayage_state_machine.hpp" + #include -#include -#include #include -#include "port_drayage_plugin/port_drayage_state_machine.h" +#include +#include namespace port_drayage_plugin { @@ -82,152 +86,124 @@ namespace port_drayage_plugin * \param op Operation enum associated with this object. */ OperationID(enum Operation op) : - _operation_enum(op) {}; + operation_enum_(op) {}; /** * \brief Getter function to obtain the Operation enum associated with this object. * \return Operation enum associated with this object. */ - OperationID::Operation get_operation_ID() const; + OperationID::Operation getOperationID() const; /** - * \brief Function to convert this object's '_operation_enum' to a human-readable string. - * \return A human-readable string representing this object's '_operation_enum'. + * \brief Function to convert this object's 'operation_enum_' to a human-readable string. + * \return A human-readable string representing this object's 'operation_enum_'. */ - std::string operation_to_string() const; + std::string operationToString() const; /** * \brief Stream operator for this object. */ friend std::ostream& operator<<(std::ostream& output, const OperationID& oid){ - return output << oid.operation_to_string(); + return output << oid.operationToString(); } /** * \brief Overloaded == operator for comparision with String objects. */ friend bool operator==(const std::string& lhs, const OperationID& rhs) { - return lhs == rhs.operation_to_string(); + return lhs == rhs.operationToString(); } private: // Data member containing this object's Operation enum value - const Operation _operation_enum = Operation::DEFAULT_OPERATION; + const Operation operation_enum_ = Operation::DEFAULT_OPERATION; }; /** * Implementation class for all the business logic of the PortDrayagePlugin - * - * Should not contain any reference to ROS publishing/subscribing or params - * and all of that data (as needed) should be pushed into it via methods. * This implementation uses lambdas and std::function to give it the ability - * to publish the outbound MobilityOperation message w/o the need for it - * to know about the ROS underlying implementation. */ class PortDrayageWorker { private: - cav_msgs::ManeuverPlanConstPtr _cur_plan; - geometry_msgs::TwistStampedConstPtr _cur_speed; - cav_msgs::RouteEventConstPtr _latest_route_event; - double _stop_speed_epsilon; - PortDrayageStateMachine _pdsm; - std::string _host_id; - std::string _host_bsm_id; - std::string _previously_completed_operation; - OperationID _pickup_operation{OperationID::PICKUP}; - OperationID _dropoff_operation{OperationID::DROPOFF}; - OperationID _enter_staging_area_operation{OperationID::ENTER_STAGING_AREA}; - OperationID _enter_port_operation{OperationID::ENTER_PORT}; - OperationID _holding_area_operation{OperationID::HOLDING_AREA}; - std::string _cmv_id; - std::string _cargo_id; // Empty if CMV is not currently carrying cargo - std::function _publish_mobility_operation; - std::function _publish_ui_instructions; - std::function _set_active_route; - std::shared_ptr _map_projector = nullptr; - bool _starting_at_staging_area; // Flag indicating CMV's first destination; 'true' indicates Staging Area Entrance; 'false' indicates Port Entrance. - bool _enable_port_drayage; // Flag to enable to port drayage operations. If false, state machine will remain in 'INACTIVE' state + std::shared_ptr latest_route_event_ = nullptr; + PortDrayageStateMachine pdsm_; + std::string previously_completed_operation_; + std::string cmv_id_; + std::string cargo_id_; // Empty if CMV is not currently carrying cargo + std::function publish_mobility_operation_; + std::function publish_ui_instructions_; + std::function)> call_set_active_route_service_; + std::shared_ptr map_projector_ = nullptr; + bool starting_at_staging_area_; // Flag indicating CMV's first destination; 'true' indicates Staging Area Entrance; 'false' indicates Port Entrance. + bool enable_port_drayage_; // Flag to enable to port drayage operations. If false, state machine will remain in 'INACTIVE' state // Data member for storing the strategy_params field of the last processed port drayage MobilityOperation message intended for this vehicle's cmv_id - std::string _previous_strategy_params; + std::string previous_strategy_params_; // Constants const std::string PORT_DRAYAGE_PLUGIN_ID = "port_drayage_plugin"; const std::string PORT_DRAYAGE_STRATEGY_ID = "carma/port_drayage"; const std::string SET_GUIDANCE_ACTIVE_SERVICE_ID = "/guidance/set_guidance_active"; + + // Clock for this object + rclcpp::Clock::SharedPtr clock_; + + // Logger interface for this object + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; public: /** * \brief Standard constructor for the PortDrayageWorker * - * \param cmv_id The Carrier Motor Vehicle ID for the host - * vehicle - * - * \param cargo_id The identification string for the cargo carried - * by the host vehicle. If no cargo is being carried this should be - * empty. - * - * \param host_id The CARMA ID string for the host vehicle - * - * \param starting_at_staging_area Flag indicating whether CMV's first - * destination is the Staging Area Entrance. If 'false, CMV's first destination - * is the Port Entrance. - * - * \param mobility_operations_publisher A lambda containing the logic - * necessary to publish a MobilityOperations message. This lambda should - * contain all the necessary ROS logic so that it does not leak into - * the implementation of this class - * - * \param stop_speed_epsilon An epsilon factor to be used when - * comparing the current vehicle's speed to 0.0 + * \param mobility_operations_publisher A function containing the logic + * necessary to publish a Mobility Operations message. * - * \param enable_port_drayage A boolean flag indicating whether port drayage - * operations are enabled or not. If false, the port drayage state machine - * will remain in an INACTIVE state. + * \param ui_instructions_publisher A function containing the logic + * necessary to publish a UI Instructions message. * - * \param call_set_active_route_client A lambda containing the logic - * necessary to call the SetActiveRoute service client. This lambda should - * contain all the necessary ROS logic so that it does not leak into - * the implementation of this class. + * \param call_set_active_route A function containing the logic + * necessary to call the SetActiveRoute service client. + */ + PortDrayageWorker(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, + rclcpp::Clock::SharedPtr clock, + std::function mobility_operations_publisher, + std::function ui_instructions_publisher, + std::function)> call_set_active_route); + + /** + * \brief Setter function to set this object's cmv_id_ string + * \param cmv_id The provided CMV ID for this object + */ + void setVehicleID(const std::string& cmv_id); + + /** + * \brief Setter function to set this object's cargo_id_ string + * \param cargo_id The provided Cargo ID for this object + */ + void setCargoID(const std::string& cargo_id); + + /** + * \brief Setter function to set this object's enable_port_drayage flag + * \param enable_port_drayage The provided boolean flag */ - PortDrayageWorker( - std::string cmv_id, - std::string cargo_id, - std::string host_id, - bool starting_at_staging_area, - std::function mobility_operations_publisher, - std::function ui_instructions_publisher, - double stop_speed_epsilon, - bool enable_port_drayage, - std::function call_set_active_route_client) : - _cmv_id(cmv_id), - _cargo_id(cargo_id), - _host_id(host_id), - _starting_at_staging_area(starting_at_staging_area), - _publish_mobility_operation(mobility_operations_publisher), - _publish_ui_instructions(ui_instructions_publisher), - _set_active_route(call_set_active_route_client), - _stop_speed_epsilon(stop_speed_epsilon), - _enable_port_drayage(enable_port_drayage) { - initialize(); - }; + void setEnablePortDrayageFlag(bool enable_port_drayage); /** - * \brief Initialize the PortDrayageWorker, setting up it's relation - * to the state machine. + * \brief Setter function to set this object's starting_at_staging_area flag + * \param starting_at_staging_area The provided boolean flag */ - void initialize(); + void setStartingAtStagingAreaFlag(bool starting_at_staging_area); /** * \brief Callback for usage by the PortDrayageStateMachine when the vehicle has arrived at a destination */ - void on_arrived_at_destination(); + void onArrivedAtDestination(); /** * \brief Callback for usage by the PortDrayageStateMachine when the vehicle has received a new destination */ - void on_received_new_destination(); + void onReceivedNewDestination(); /** * \brief Create a SetActiveRoute service request to set a new active route for the system based on @@ -236,7 +212,7 @@ namespace port_drayage_plugin * \param dest_latitude The destination point's latitude * \param dest_longitude The destination point's longitude */ - cav_srvs::SetActiveRoute compose_set_active_route_request(boost::optional dest_latitude, boost::optional dest_longitude) const; + std::shared_ptr composeSetActiveRouteRequest(boost::optional dest_latitude, boost::optional dest_longitude) const; /** * \brief Creates a UIInstructions message that can be used to create a pop-up on the Web UI to notify a user that a new @@ -244,41 +220,31 @@ namespace port_drayage_plugin * \param current_operation The 'operation' identifier associated with the latest received and processed Port Drayage MobilityOperation message. * \param previous_operation The previously completed 'operation' identifier. This is an empty string if no 'operation' was previously completed. */ - cav_msgs::UIInstructions compose_ui_instructions(const std::string& current_operation, const std::string& previous_operation); + carma_msgs::msg::UIInstructions composeUIInstructions(const std::string& current_operation, const std::string& previous_operation); /** * \brief Assemble the current dataset into a MobilityOperations * message with a JSON formatted body containing CMV ID and cargo ID */ - cav_msgs::MobilityOperation compose_arrival_message() const; - - /** - * \brief Set the current plan from the arbitrator - */ - void set_maneuver_plan(const cav_msgs::ManeuverPlanConstPtr& plan); - - /** - * \brief Set the current speed as measured by the vehicle's sensors - */ - void set_current_speed(const geometry_msgs::TwistStampedConstPtr& speed); + carma_v2x_msgs::msg::MobilityOperation composeArrivalMessage() const; /** * \brief Callback for map projection string to define lat/lon <--> map conversion * \param msg The proj string defining the projection. */ - void on_new_georeference(const std_msgs::StringConstPtr& msg); + void onNewGeoreference(std_msgs::msg::String::UniquePtr msg); /** * \brief Callback for the pose subscriber. The pose will be converted into lat/lon and stored locally. * \param msg Latest pose message */ - void on_new_pose(const geometry_msgs::PoseStampedConstPtr& msg); + void onNewPose(geometry_msgs::msg::PoseStamped::UniquePtr msg); /** * \brief Callback to process a received MobilityOperation message * \param msg a received MobilityOperation message */ - void on_inbound_mobility_operation(const cav_msgs::MobilityOperationConstPtr& msg); + void onInboundMobilityOperation(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg); /** * \brief Method to update worker's cargo-related data members depending on whether @@ -286,7 +252,7 @@ namespace port_drayage_plugin * \param previous_port_drayage_msg The contents of the previously received MobilityOperation * port drayage message for this CMV stored in a PortDrayageMobilityOperationMsg object. */ - void update_cargo_information_after_action_completion(const PortDrayageMobilityOperationMsg& previous_port_drayage_msg); + void updateCargoInformationAfterActionCompletion(const PortDrayageMobilityOperationMsg& previous_port_drayage_msg); /** * \brief Function to help parse the text included in an inbound MobilityOperation message's @@ -294,47 +260,30 @@ namespace port_drayage_plugin * with strategy type 'carma/port_drayage'. Stores the parsed information in _latest_mobility_operation_msg. * \param mobility_operation_strategy_params the strategy_params field of a MobilityOperation message */ - void mobility_operation_message_parser(std::string mobility_operation_strategy_params); + void mobilityOperationMessageParser(std::string mobility_operation_strategy_params); /** * \brief Callback to process the current status of the guidance state machine. * \param msg a received GuidanceState message */ - void on_guidance_state(const cav_msgs::GuidanceStateConstPtr& msg); + void onGuidanceState(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg); /** * \brief Callback to process each Route Event * \param msg a received RouteEvent message */ - void on_route_event(const cav_msgs::RouteEventConstPtr& msg); + void onRouteEvent(const carma_planning_msgs::msg::RouteEvent::UniquePtr msg); /** * \brief Get the current state of the port drayage state machine * \return The current state value of the port drayage state machine */ - PortDrayageState get_port_drayage_state(); - - /** - * \brief Spin and process data - */ - bool spin(); - - /** - * \brief Check to see if the vehicle has stopped under the command of the - * Port Drayage Plugin. - * - * \param plan The current maneuver plan - * \param speed The current vehicle speed - * - * \return True if the vehicle has been stopped by the PDP, false o.w. - */ - bool check_for_stop(const cav_msgs::ManeuverPlanConstPtr& plan, const geometry_msgs::TwistStampedConstPtr& speed) const; + PortDrayageState getPortDrayageState(); // PortDrayageMobilityOperationMsg object for storing strategy_params data of a received port drayage MobilityOperation message intended for this vehicle's cmv_id - PortDrayageMobilityOperationMsg _latest_mobility_operation_msg; + PortDrayageMobilityOperationMsg latest_mobility_operation_msg_; // LatLonCoordinate object for storing the vehicle's current gps latitude/longitude coordinates - LatLonCoordinate _current_gps_position; - + LatLonCoordinate current_gps_position_; }; -} // namespace port_drayage_plugin +} // namespace port_drayage_plugin \ No newline at end of file diff --git a/port_drayage_plugin/launch/port_drayage_plugin.launch b/port_drayage_plugin/launch/port_drayage_plugin.launch deleted file mode 100644 index 0d3c537ea1..0000000000 --- a/port_drayage_plugin/launch/port_drayage_plugin.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - diff --git a/port_drayage_plugin/launch/port_drayage_plugin_launch.py b/port_drayage_plugin/launch/port_drayage_plugin_launch.py new file mode 100644 index 0000000000..cfdd7585ee --- /dev/null +++ b/port_drayage_plugin/launch/port_drayage_plugin_launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file can be used to launch the CARMA port_drayage_plugin_node. +Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('port_drayage_plugin'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='port_drayage_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='port_drayage_plugin', + plugin='port_drayage_plugin::PortDrayagePlugin', + name='port_drayage_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/port_drayage_plugin/package.xml b/port_drayage_plugin/package.xml index a2f81eb02e..17cb4ee506 100644 --- a/port_drayage_plugin/package.xml +++ b/port_drayage_plugin/package.xml @@ -1,21 +1,47 @@ + + + port_drayage_plugin - 1.0.0 - CARMA plugin to support port drayage applications for CARMA Freight - CARMA Team - MIT - - catkin - carma_utils - cav_msgs - cav_srvs - roscpp - std_msgs - carma_wm + 4.0.0 + The port_drayage_plugin package + + carma + + Apache 2.0 + + ament_cmake carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + carma_planning_msgs + carma_v2x_msgs + carma_msgs + geometry_msgs + lanelet2_extension + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros - + ament_cmake diff --git a/port_drayage_plugin/src/port_drayage_plugin_main.cpp b/port_drayage_plugin/src/main.cpp similarity index 51% rename from port_drayage_plugin/src/port_drayage_plugin_main.cpp rename to port_drayage_plugin/src/main.cpp index 54c9439e69..1e5ff5d19e 100644 --- a/port_drayage_plugin/src/port_drayage_plugin_main.cpp +++ b/port_drayage_plugin/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * 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 @@ -14,19 +14,20 @@ * the License. */ -// @SONAR_STOP@ -#include -#include -#include -#include "port_drayage_plugin/port_drayage_plugin.h" +#include +#include "port_drayage_plugin/port_drayage_plugin.hpp" -int main(int argc, char** argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "port_drayage_plugin"); - std::shared_ptr nh = std::make_shared(""); - std::shared_ptr pnh = std::make_shared("~"); - port_drayage_plugin::PortDrayagePlugin pdp{nh, pnh}; - return pdp.run(); -} + 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(); -// @SONAR_START \ No newline at end of file + rclcpp::shutdown(); + + return 0; +} diff --git a/port_drayage_plugin/src/port_drayage_plugin.cpp b/port_drayage_plugin/src/port_drayage_plugin.cpp index fd26ac908e..4141bff8e3 100644 --- a/port_drayage_plugin/src/port_drayage_plugin.cpp +++ b/port_drayage_plugin/src/port_drayage_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2022 LEIDOS. + * Copyright (C) 2020-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 @@ -13,290 +13,139 @@ * License for the specific language governing permissions and limitations under * the License. */ - -#include "port_drayage_plugin/port_drayage_plugin.h" -#include "port_drayage_plugin/port_drayage_worker.h" +#include "port_drayage_plugin/port_drayage_plugin.hpp" namespace port_drayage_plugin { - - // @SONAR_STOP@ - int PortDrayagePlugin::run() { - if (_nh == nullptr || _pnh == nullptr) { - ROS_ERROR("Port Drayage Plugin not properly initialized, node handles are null!"); - return -1; - } - - // Read in configuration parameters - double speed_epsilon = _pnh->param("stop_speed_epsilon", 1.0); - declaration = _pnh->param("declaration", 1.0); - std::string cargo_id; - _pnh->param("cargo_id", cargo_id, "UNDEFINED-CARGO-ID"); - std::string host_id; - _pnh->param("host_id", host_id, "UNDEFINED-HOST-ID"); - bool starting_at_staging_area; - _pnh->param("starting_at_staging_area", starting_at_staging_area, true); - bool enable_port_drayage; - _pnh->param("enable_port_drayage", enable_port_drayage, false); - std::string cmv_id; - _pnh->getParam("/vehicle_id", cmv_id); - - ros::Publisher outbound_mob_op = _nh->advertise("outgoing_mobility_operation", 5); - _outbound_mobility_operations_publisher = std::make_shared(outbound_mob_op); - - ros::Publisher ui_instructions_pub = _nh->advertise("ui_instructions", 5); - _ui_instructions_publisher = std::make_shared(ui_instructions_pub); - - _set_active_route_client = _nh->serviceClient("/guidance/set_active_route"); - - PortDrayageWorker pdw{ - cmv_id, - cargo_id, - host_id, - starting_at_staging_area, - [this](cav_msgs::MobilityOperation msg) { - _outbound_mobility_operations_publisher->publish(msg); - }, - [this](cav_msgs::UIInstructions msg) { - _ui_instructions_publisher->publish(msg); - }, - speed_epsilon, - enable_port_drayage, - std::bind(&PortDrayagePlugin::call_set_active_route_client, this, std::placeholders::_1) - }; - - ros::Subscriber maneuver_sub = _nh->subscribe("final_maneuver_plan", 5, - [&](const cav_msgs::ManeuverPlanConstPtr& plan) { - pdw.set_maneuver_plan(plan); - }); - _maneuver_plan_subscriber = std::make_shared(maneuver_sub); - - ros::Subscriber twist_sub = _nh->subscribe("current_velocity", 5, - [&](const geometry_msgs::TwistStampedConstPtr& speed) { - pdw.set_current_speed(speed); - _cur_speed = speed->twist; - }); - _cur_speed_subscriber = std::make_shared(twist_sub); - - plan_maneuver_srv_ = _nh->advertiseService("strategic_plan/plan_maneuvers", &PortDrayagePlugin::plan_maneuver_cb, this); - - ros::Subscriber pose_sub = _nh->subscribe("current_pose", 5, - [&](const geometry_msgs::PoseStampedConstPtr& pose) { - curr_pose_ = std::make_shared(*pose); - pdw.on_new_pose(pose); - }); - - _pose_subscriber = std::make_shared(pose_sub); - - ros::Subscriber inbound_mobility_operation_sub = _nh->subscribe("incoming_mobility_operation", 5, - [&](const cav_msgs::MobilityOperationConstPtr& mobility_msg){ - pdw.on_inbound_mobility_operation(mobility_msg); - }); - - _inbound_mobility_operation_subscriber = std::make_shared(inbound_mobility_operation_sub); - - ros::Subscriber georeference_sub = _nh->subscribe("georeference", 1, - [&](const std_msgs::StringConstPtr& georeference_msg) { - pdw.on_new_georeference(georeference_msg); - }); - - _georeference_subscriber = std::make_shared(georeference_sub); - - ros::Subscriber guidance_state_sub = _nh->subscribe("guidance_state", 5, - [&](const cav_msgs::GuidanceStateConstPtr& guidance_state) { - pdw.on_guidance_state(guidance_state); - }); - - _guidance_state_subscriber = std::make_shared(guidance_state_sub); - - ros::Subscriber route_event_sub = _nh->subscribe("route_event", 5, - [&](const cav_msgs::RouteEventConstPtr& route_event) { - pdw.on_route_event(route_event); - }); - - _route_event_subscriber = std::make_shared(route_event_sub); - - ros::Timer discovery_pub_timer_ = _nh->createTimer( - ros::Duration(ros::Rate(10.0)), - [&pdw](const auto&) { pdw.spin(); }); - - ros::CARMANodeHandle::spin(); - - return 0; + namespace std_ph = std::placeholders; + + PortDrayagePlugin::PortDrayagePlugin(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options), + pdw_(get_node_logging_interface(), get_clock(), + std::bind(&PortDrayagePlugin::publishMobilityOperation, this, std_ph::_1), + std::bind(&PortDrayagePlugin::publishUIInstructions, this, std_ph::_1), + std::bind(&PortDrayagePlugin::callSetActiveRouteClient, this, std_ph::_1)) + { + // Create initial config + config_ = Config(); + + // Declare parameters + config_.cmv_id = declare_parameter("vehicle_id", config_.cmv_id); + config_.cargo_id = declare_parameter("cargo_id", config_.cargo_id); + config_.enable_port_drayage = declare_parameter("enable_port_drayage", config_.enable_port_drayage); + config_.starting_at_staging_area = declare_parameter("starting_at_staging_area", config_.starting_at_staging_area); + } + + rcl_interfaces::msg::SetParametersResult PortDrayagePlugin::parameter_update_callback(const std::vector ¶meters) + { + auto error = update_params({ + {"vehicle_id", config_.cmv_id}, + {"cargo_id", config_.cargo_id}}, parameters); + auto error_2 = update_params({ + {"enable_port_drayage", config_.enable_port_drayage}, + {"starting_at_staging_area", config_.starting_at_staging_area}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error && !error_2; + + if (result.successful) { + // Set PortDrayageWorker parameters + pdw_.setVehicleID(config_.cmv_id); + pdw_.setCargoID(config_.cargo_id); + pdw_.setEnablePortDrayageFlag(config_.enable_port_drayage); + pdw_.setStartingAtStagingAreaFlag(config_.starting_at_staging_area); } - bool PortDrayagePlugin::call_set_active_route_client(cav_srvs::SetActiveRoute req){ - if(_set_active_route_client.call(req)){ - if(req.response.error_status == cav_srvs::SetActiveRouteResponse::NO_ERROR){ - ROS_DEBUG_STREAM("Route Generation succeeded for Set Active Route service call."); - return true; - } - else{ - ROS_DEBUG_STREAM("Route Generation failed for Set Active Route service call."); - return false; - } - } - else{ - ROS_DEBUG_STREAM("Set Active Route service call was not successful."); - return false; - } + return result; + } + + carma_ros2_utils::CallbackReturn PortDrayagePlugin::handle_on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO_STREAM(get_logger(), "port_drayage_plugin trying to configure"); + + // Reset config + config_ = Config(); + + // Load parameters + get_parameter("vehicle_id", config_.cmv_id); + get_parameter("cargo_id", config_.cargo_id); + get_parameter("enable_port_drayage", config_.enable_port_drayage); + get_parameter("starting_at_staging_area", config_.starting_at_staging_area); + + RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&PortDrayagePlugin::parameter_update_callback, this, std_ph::_1)); + + // Setup subscribers + pose_subscriber_ = create_subscription("current_pose", 5, + std::bind(&PortDrayageWorker::onNewPose, &pdw_, std_ph::_1)); + inbound_mobility_operation_subscriber_ = create_subscription("incoming_mobility_operation", 5, + std::bind(&PortDrayageWorker::onInboundMobilityOperation, &pdw_, std_ph::_1)); + guidance_state_subscriber_ = create_subscription ("guidance_state", 5, + std::bind(&PortDrayageWorker::onGuidanceState, &pdw_, std_ph::_1)); + route_event_subscriber_ = create_subscription("route_event", 5, + std::bind(&PortDrayageWorker::onRouteEvent, &pdw_, std_ph::_1)); + georeference_subscriber_ = create_subscription("georeference", 1, + std::bind(&PortDrayageWorker::onNewGeoreference, &pdw_, std_ph::_1)); + + // Setup publishers + outbound_mobility_operations_publisher_ = create_publisher("outgoing_mobility_operation", 5); + ui_instructions_publisher_ = create_publisher("ui_instructions", 5); + + // Setup service clients + set_active_route_client_ = create_client("set_active_route"); + + // Set PortDrayageWorker variables + pdw_.setVehicleID(config_.cmv_id); + pdw_.setCargoID(config_.cargo_id); + pdw_.setEnablePortDrayageFlag(config_.enable_port_drayage); + pdw_.setStartingAtStagingAreaFlag(config_.starting_at_staging_area); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + bool PortDrayagePlugin::callSetActiveRouteClient(std::shared_ptr req) + { + // Send request to set_active_route service + auto route_result = set_active_route_client_->async_send_request(req); + + // Wait for response from service call + auto future_status = route_result.wait_for(std::chrono::milliseconds(100)); + + if (future_status == std::future_status::ready) { + if(route_result.get()->error_status == carma_planning_msgs::srv::SetActiveRoute::Response::NO_ERROR){ + RCLCPP_DEBUG_STREAM(get_logger(), "Route Generation succeeded for Set Active Route service call."); + return true; + } + else{ + RCLCPP_DEBUG_STREAM(get_logger(), "Route Generation failed for Set Active Route service call."); + return false; + } + } + else { + // No response was received after making the service call + RCLCPP_DEBUG_STREAM(get_logger(), "Set Active Route service call was not successful."); + return false; } - bool PortDrayagePlugin::plan_maneuver_cb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp){ - - if(curr_pose_ == nullptr) { - return false; - } - - lanelet::BasicPoint2d current_loc(curr_pose_->pose.position.x, curr_pose_->pose.position.y); - - if(wm_ == nullptr) { - return false; - } - - auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 1); - - if(current_lanelets.size() == 0) { - ROS_WARN_STREAM("Cannot find any lanelet in map!"); - return false; - } - - auto current_lanelet = current_lanelets[0]; - auto stop_rules = current_lanelet.second.regulatoryElementsAs(); - double stop_rule_downtrack = 0; - - if(stop_rules.empty()) { - return false; - } - else { - auto stop_rule_elem = stop_rules.front(); - auto stop_line_vector = stop_rule_elem->stopAndWaitLine(); - auto stop_line = lanelet::traits::to2D(stop_line_vector.front()); - auto point = lanelet::traits::to2D(stop_line.front()); - auto pos = carma_wm::geometry::trackPos(current_lanelet.second, point); - stop_rule_downtrack = pos.downtrack; - } - - double current_loc_downtrack = wm_->routeTrackPos(current_loc).downtrack; - - double speed_progress = _cur_speed.linear.x; - double current_progress = 0; - - if(current_loc_downtrack < stop_rule_downtrack) - { - - ros::Time start_time; - - if(req.prior_plan.maneuvers.size() > 0) { - switch(req.prior_plan.maneuvers.back().type) { - case cav_msgs::Maneuver::LANE_FOLLOWING : - start_time = req.prior_plan.maneuvers.back().lane_following_maneuver.end_time; - current_progress = req.prior_plan.maneuvers.back().lane_following_maneuver.end_dist; - break; - case cav_msgs::Maneuver::LANE_CHANGE : - start_time = req.prior_plan.maneuvers.back().lane_change_maneuver.end_time; - current_progress = req.prior_plan.maneuvers.back().lane_change_maneuver.end_dist; - break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT : - start_time = req.prior_plan.maneuvers.back().intersection_transit_straight_maneuver.end_time; - current_progress = req.prior_plan.maneuvers.back().intersection_transit_straight_maneuver.end_dist; - break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN : - start_time = req.prior_plan.maneuvers.back().intersection_transit_left_turn_maneuver.end_time; - current_progress = req.prior_plan.maneuvers.back().intersection_transit_left_turn_maneuver.end_dist; - break; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN : - start_time = req.prior_plan.maneuvers.back().intersection_transit_right_turn_maneuver.end_time; - current_progress = req.prior_plan.maneuvers.back().intersection_transit_right_turn_maneuver.end_dist; - break; - case cav_msgs::Maneuver::STOP_AND_WAIT : - start_time = req.prior_plan.maneuvers.back().stop_and_wait_maneuver.end_time; - current_progress = req.prior_plan.maneuvers.back().stop_and_wait_maneuver.end_dist; - break; - } - } else { - start_time = ros::Time::now(); - current_progress = current_loc_downtrack; - } - - double end_dist = stop_rule_downtrack; - - double estimated_distance_to_stop = estimate_distance_to_stop(speed_progress,declaration); - double estimated_time_to_stop = estimate_time_to_stop(estimated_distance_to_stop,speed_progress); - - double lane_following_distance = stop_rule_downtrack - estimated_distance_to_stop; - double stop_and_wait_distance = estimated_distance_to_stop; - - resp.new_plan.maneuvers.push_back( - compose_lane_following_maneuver_message(current_progress, - current_progress + lane_following_distance, - speed_progress, - speed_progress, - current_lanelet.second.id(), - start_time)); - - resp.new_plan.maneuvers.push_back( - compose_stop_and_wait_maneuver_message(current_progress + lane_following_distance, - end_dist, - speed_progress, - 0.0, - current_lanelet.second.id(), - start_time, - estimated_time_to_stop)); - } - - if(resp.new_plan.maneuvers.size() == 0) - { - ROS_WARN_STREAM("Cannot plan maneuver because no route is found"); - } - - return true; - }; + } - cav_msgs::Maneuver PortDrayagePlugin::compose_stop_and_wait_maneuver_message(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time time, double time_to_stop) - { - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::STOP_AND_WAIT; - maneuver_msg.stop_and_wait_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; - maneuver_msg.stop_and_wait_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.stop_and_wait_maneuver.parameters.planning_tactical_plugin = "stop_and_wait_plugin"; - maneuver_msg.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "PortDrayageWorkerPlugin"; - maneuver_msg.stop_and_wait_maneuver.start_dist = current_dist; - maneuver_msg.stop_and_wait_maneuver.start_speed = current_speed; - maneuver_msg.stop_and_wait_maneuver.start_time = time; - maneuver_msg.stop_and_wait_maneuver.end_dist = end_dist; - maneuver_msg.stop_and_wait_maneuver.end_time = time + ros::Duration(std::max(15.0,time_to_stop)); - maneuver_msg.stop_and_wait_maneuver.starting_lane_id = std::to_string(lane_id); - maneuver_msg.stop_and_wait_maneuver.ending_lane_id = std::to_string(lane_id); - return maneuver_msg; - } + void PortDrayagePlugin::publishMobilityOperation(const carma_v2x_msgs::msg::MobilityOperation& msg) + { + outbound_mobility_operations_publisher_->publish(msg); + } - cav_msgs::Maneuver PortDrayagePlugin::compose_lane_following_maneuver_message(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time time) - { - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING; - maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; - maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "InlaneCruisingPlugin"; - maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "route_following_plugin"; - maneuver_msg.lane_following_maneuver.start_dist = current_dist; - maneuver_msg.lane_following_maneuver.start_speed = current_speed; - maneuver_msg.lane_following_maneuver.start_time = time; - maneuver_msg.lane_following_maneuver.end_dist = end_dist; - maneuver_msg.lane_following_maneuver.end_speed = target_speed; - // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet - maneuver_msg.lane_following_maneuver.end_time = time + ros::Duration((end_dist - current_dist) / (0.5 * (current_speed + target_speed))); - maneuver_msg.lane_following_maneuver.lane_ids = { std::to_string(lane_id) }; - return maneuver_msg; - } - // @SONAR_START@ + void PortDrayagePlugin::publishUIInstructions(const carma_msgs::msg::UIInstructions& msg) + { + ui_instructions_publisher_->publish(msg); + } - double estimate_distance_to_stop(double v, double a) { - return (v*v)/2*a; - } +} // port_drayage_plugin - double estimate_time_to_stop(double d, double v) { - return 2*d/v; - }; +#include "rclcpp_components/register_node_macro.hpp" -} // namespace port_drayage_plugin \ No newline at end of file +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(port_drayage_plugin::PortDrayagePlugin) diff --git a/port_drayage_plugin/src/port_drayage_state_machine.cpp b/port_drayage_plugin/src/port_drayage_state_machine.cpp index fc62b68823..2663cd7ebb 100644 --- a/port_drayage_plugin/src/port_drayage_state_machine.cpp +++ b/port_drayage_plugin/src/port_drayage_state_machine.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2020-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 @@ -14,67 +14,65 @@ * the License. */ -#include -#include "port_drayage_plugin/port_drayage_state_machine.h" -#include +#include "port_drayage_plugin/port_drayage_state_machine.hpp" namespace port_drayage_plugin { - void PortDrayageStateMachine::process_event(PortDrayageEvent event) { - switch (_state) + void PortDrayageStateMachine::processEvent(PortDrayageEvent event) { + switch (state_) { case PortDrayageState::INACTIVE: if (event == PortDrayageEvent::DRAYAGE_START) { - _state = PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION; + state_ = PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION; } break; case PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION: if (event == PortDrayageEvent::ARRIVED_AT_DESTINATION) { - if (_on_arrived_at_destination) { - _on_arrived_at_destination(); + if (on_arrived_at_destination_) { + on_arrived_at_destination_(); } - _state = PortDrayageState::AWAITING_DIRECTION; + state_ = PortDrayageState::AWAITING_DIRECTION; } break; case PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION: if (event == PortDrayageEvent::ARRIVED_AT_DESTINATION) { - if (_on_arrived_at_destination) { - _on_arrived_at_destination(); + if (on_arrived_at_destination_) { + on_arrived_at_destination_(); } - _state = PortDrayageState::AWAITING_DIRECTION; + state_ = PortDrayageState::AWAITING_DIRECTION; } break; case PortDrayageState::AWAITING_DIRECTION: if (event == PortDrayageEvent::RECEIVED_NEW_DESTINATION) { - _state = PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION; - if (_on_received_new_destination) { - _on_received_new_destination(); + state_ = PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION; + if (on_received_new_destination_) { + on_received_new_destination_(); } } break; default: - ROS_ERROR_STREAM("Unhandled port drayage state: " << _state << "!"); + RCLCPP_ERROR_STREAM(logger_->get_logger(), "Unhandled port drayage state: " << state_ << "!"); throw std::invalid_argument("Unhandled port drayage state"); } } - PortDrayageState PortDrayageStateMachine::get_state() const { - return _state; + PortDrayageState PortDrayageStateMachine::getState() const { + return state_; } - void PortDrayageStateMachine::set_on_system_startup_callback(const std::function &cb) { - _on_system_startup = cb; + void PortDrayageStateMachine::setOnSystemStartupCallback(const std::function &cb) { + on_system_startup_ = cb; } - void PortDrayageStateMachine::set_on_received_new_destination_callback(const std::function &cb) { - _on_received_new_destination = cb; + void PortDrayageStateMachine::setOnReceivedNewDestinationCallback(const std::function &cb) { + on_received_new_destination_ = cb; } - void PortDrayageStateMachine::set_on_arrived_at_destination_callback(const std::function &cb) { - _on_arrived_at_destination = cb; + void PortDrayageStateMachine::setOnArrivedAtDestinationCallback(const std::function &cb) { + on_arrived_at_destination_ = cb; } - void PortDrayageStateMachine::set_on_drayage_completed_callback(const std::function &cb) { - _on_drayage_completed = cb; + void PortDrayageStateMachine::setOnDrayageCompletedCallback(const std::function &cb) { + on_drayage_completed_ = cb; } -} +} \ No newline at end of file diff --git a/port_drayage_plugin/src/port_drayage_worker.cpp b/port_drayage_plugin/src/port_drayage_worker.cpp index 1934e3b2ab..9a5203a080 100644 --- a/port_drayage_plugin/src/port_drayage_worker.cpp +++ b/port_drayage_plugin/src/port_drayage_worker.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2022 LEIDOS. + * Copyright (C) 2020-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 @@ -14,21 +14,18 @@ * the License. */ -#include "port_drayage_plugin/port_drayage_worker.h" -#include "ros/ros.h" -#include -#include +#include "port_drayage_plugin/port_drayage_worker.hpp" namespace port_drayage_plugin { - OperationID::Operation OperationID::get_operation_ID() const { - return _operation_enum; + OperationID::Operation OperationID::getOperationID() const { + return operation_enum_; } - std::string OperationID::operation_to_string() const { + std::string OperationID::operationToString() const { // Convert operation enum into a human-readable string - switch(_operation_enum) { + switch(operation_enum_) { case Operation::PICKUP: return "PICKUP"; case Operation::DROPOFF: return "DROPOFF"; case Operation::ENTER_STAGING_AREA: return "ENTER_STAGING_AREA"; @@ -38,110 +35,99 @@ namespace port_drayage_plugin case Operation::PORT_CHECKPOINT: return "PORT_CHECKPOINT"; case Operation::HOLDING_AREA: return "HOLDING_AREA"; default: - ROS_WARN_STREAM("Conversion of an unsupported operation enum value to a string."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("OperationID"), "Conversion of an unsupported operation enum value to a string."); return "UNSUPPORTED_OPERATION_ID"; } } - bool PortDrayageWorker::check_for_stop(const cav_msgs::ManeuverPlanConstPtr& plan, const geometry_msgs::TwistStampedConstPtr& speed) const { - if (plan == nullptr || speed == nullptr) { - ROS_DEBUG("Checking for stop when PortDrayagePlugin not properly initialized. Speed or plan is null"); - return false; - } - - if (plan->maneuvers.size() > 0 && plan->maneuvers[0].type == cav_msgs::Maneuver::STOP_AND_WAIT) { - cav_msgs::Maneuver stop_maneuver = _cur_plan->maneuvers[0]; - if (stop_maneuver.stop_and_wait_maneuver.parameters.planning_strategic_plugin == PORT_DRAYAGE_PLUGIN_ID) { - double longitudinal_speed = speed->twist.linear.x; - if (longitudinal_speed <= _stop_speed_epsilon) { - return true; - } - } - } - - return false; + PortDrayageWorker::PortDrayageWorker(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, + rclcpp::Clock::SharedPtr clock, + std::function mobility_operations_publisher, + std::function ui_instructions_publisher, + std::function)> call_set_active_route) + : logger_(logger), clock_(clock), + publish_mobility_operation_(mobility_operations_publisher), + publish_ui_instructions_(ui_instructions_publisher), + call_set_active_route_service_(call_set_active_route), + pdsm_(logger) + { + pdsm_.setOnArrivedAtDestinationCallback(std::bind(&PortDrayageWorker::onArrivedAtDestination, this)); + pdsm_.setOnReceivedNewDestinationCallback(std::bind(&PortDrayageWorker::onReceivedNewDestination, this)); } - // @SONAR_STOP@ - bool PortDrayageWorker::spin() { - if (check_for_stop(_cur_plan, _cur_speed)) { - _pdsm.process_event(PortDrayageEvent::ARRIVED_AT_DESTINATION); - } - - return true; + void PortDrayageWorker::setVehicleID(const std::string& cmv_id) { + cmv_id_ = cmv_id; } - - void PortDrayageWorker::set_maneuver_plan(const cav_msgs::ManeuverPlanConstPtr& plan) { - _cur_plan = plan; + + void PortDrayageWorker::setCargoID(const std::string& cargo_id) { + cargo_id_ = cargo_id; } - void PortDrayageWorker::set_current_speed(const geometry_msgs::TwistStampedConstPtr& speed) { - _cur_speed = speed; + void PortDrayageWorker::setEnablePortDrayageFlag(bool enable_port_drayage) { + enable_port_drayage_ = enable_port_drayage; } - void PortDrayageWorker::initialize() { - _pdsm.set_on_arrived_at_destination_callback(std::bind(&PortDrayageWorker::on_arrived_at_destination, this)); - _pdsm.set_on_received_new_destination_callback(std::bind(&PortDrayageWorker::on_received_new_destination, this)); + void PortDrayageWorker::setStartingAtStagingAreaFlag(bool starting_at_staging_area) { + starting_at_staging_area_ = starting_at_staging_area; } - // @SONAR_START@ - void PortDrayageWorker::on_arrived_at_destination() { - cav_msgs::MobilityOperation msg = compose_arrival_message(); - _publish_mobility_operation(msg); + void PortDrayageWorker::onArrivedAtDestination() { + carma_v2x_msgs::msg::MobilityOperation msg = composeArrivalMessage(); + publish_mobility_operation_(msg); } - void PortDrayageWorker::on_received_new_destination() { + void PortDrayageWorker::onReceivedNewDestination() { // Populate the service request with the destination coordinates from the last received port drayage mobility operation message - cav_srvs::SetActiveRoute route_req = compose_set_active_route_request(_latest_mobility_operation_msg.dest_latitude, _latest_mobility_operation_msg.dest_longitude); + auto route_req = composeSetActiveRouteRequest(latest_mobility_operation_msg_.dest_latitude, latest_mobility_operation_msg_.dest_longitude); // Call service client to set the new active route - bool is_route_generation_successful = _set_active_route(route_req); + bool is_route_generation_successful = call_set_active_route_service_(route_req); if (is_route_generation_successful) { // Publish UI Instructions to trigger a pop-up on the Web UI for the user to engage on the newly received route if desired - cav_msgs::UIInstructions ui_instructions_msg = compose_ui_instructions(_latest_mobility_operation_msg.operation, _previously_completed_operation); - _publish_ui_instructions(ui_instructions_msg); + carma_msgs::msg::UIInstructions ui_instructions_msg = composeUIInstructions(latest_mobility_operation_msg_.operation, previously_completed_operation_); + publish_ui_instructions_(ui_instructions_msg); } else { // Throw exception if route generation was not successful - ROS_DEBUG_STREAM("Route generation failed. Routing could not be completed."); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Route generation failed. Routing could not be completed."); throw std::invalid_argument("Route generation failed. Routing could not be completed."); } } - cav_srvs::SetActiveRoute PortDrayageWorker::compose_set_active_route_request(boost::optional dest_latitude, boost::optional dest_longitude) const { - cav_srvs::SetActiveRoute route_req; + std::shared_ptr PortDrayageWorker::composeSetActiveRouteRequest(boost::optional dest_latitude, boost::optional dest_longitude) const { + auto route_req = std::make_shared(); if (dest_latitude && dest_longitude) { - route_req.request.choice = cav_srvs::SetActiveRouteRequest::DESTINATION_POINTS_ARRAY; - route_req.request.route_id = _latest_mobility_operation_msg.operation; + route_req->choice = carma_planning_msgs::srv::SetActiveRoute::Request::DESTINATION_POINTS_ARRAY; + route_req->route_id = latest_mobility_operation_msg_.operation; - cav_msgs::Position3D destination_point; - destination_point.latitude = *_latest_mobility_operation_msg.dest_latitude; - destination_point.longitude = *_latest_mobility_operation_msg.dest_longitude; + carma_v2x_msgs::msg::Position3D destination_point; + destination_point.latitude = *latest_mobility_operation_msg_.dest_latitude; + destination_point.longitude = *latest_mobility_operation_msg_.dest_longitude; destination_point.elevation_exists = false; - route_req.request.destination_points.push_back(destination_point); + route_req->destination_points.push_back(destination_point); } else { - ROS_DEBUG_STREAM("No destination points were received. Routing could not be completed."); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "No destination points were received. Routing could not be completed."); throw std::invalid_argument("No destination points were received. Routing could not be completed"); } return route_req; } - cav_msgs::UIInstructions PortDrayageWorker::compose_ui_instructions(const std::string& current_operation, const std::string& previous_operation) { + carma_msgs::msg::UIInstructions PortDrayageWorker::composeUIInstructions(const std::string& current_operation, const std::string& previous_operation) { // Create the text that will be displayed in the Web UI popup std::string popup_text = ""; // Add text that indicates the previous action was completed (if it was a pickup or dropoff action) - if (previous_operation == _pickup_operation) { + if (previous_operation == OperationID::PICKUP) { popup_text += "The pickup action was completed successfully. "; } - else if (previous_operation == _dropoff_operation) { + else if (previous_operation == OperationID::DROPOFF) { popup_text += "The dropoff action was completed successfully. "; } - else if (previous_operation == _holding_area_operation) { + else if (previous_operation == OperationID::HOLDING_AREA) { popup_text += "The inspection was completed successfully. "; } @@ -151,96 +137,97 @@ namespace port_drayage_plugin "disengaged."; // Create and populate the UI Instructions message - cav_msgs::UIInstructions ui_instructions_msg; - ui_instructions_msg.stamp = ros::Time::now(); + carma_msgs::msg::UIInstructions ui_instructions_msg; + ui_instructions_msg.stamp = clock_->now(); ui_instructions_msg.msg = popup_text; - ui_instructions_msg.type = cav_msgs::UIInstructions::ACK_REQUIRED; // The popup will be displayed until the user interacts with it + ui_instructions_msg.type = carma_msgs::msg::UIInstructions::ACK_REQUIRED; // The popup will be displayed until the user interacts with it ui_instructions_msg.response_service = SET_GUIDANCE_ACTIVE_SERVICE_ID; return ui_instructions_msg; } - cav_msgs::MobilityOperation PortDrayageWorker::compose_arrival_message() const { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation PortDrayageWorker::composeArrivalMessage() const { + carma_v2x_msgs::msg::MobilityOperation msg; msg.m_header.plan_id = ""; - msg.m_header.sender_id = _host_id; - msg.m_header.sender_bsm_id = _host_bsm_id; + msg.m_header.sender_id = cmv_id_; msg.m_header.recipient_id = ""; - msg.m_header.timestamp = ros::Time::now().toNSec(); + msg.m_header.timestamp = clock_->now().nanoseconds()/1000000; msg.strategy = PORT_DRAYAGE_STRATEGY_ID; // Encode JSON with Boost Property Tree using boost::property_tree::ptree; ptree pt; - pt.put("cmv_id", _cmv_id); + pt.put("cmv_id", cmv_id_); // Add current vehicle location (latitude and longitude) ptree location; - location.put("latitude", _current_gps_position.latitude); - location.put("longitude", _current_gps_position.longitude); + location.put("latitude", current_gps_position_.latitude); + location.put("longitude", current_gps_position_.longitude); pt.put_child("location", location); // Add flag to indicate whether CMV is carring cargo - pt.put("cargo", _cargo_id != ""); + pt.put("cargo", cargo_id_ != ""); // If CMV has arrived at its initial destination, assign 'operation' field for its initial arrival message - if (_pdsm.get_state() == PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION) { + if (pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION) { // The CMV's initial destination is either the Staging Area Entrance or the Port Entrance - if (_starting_at_staging_area) { - pt.put("operation", _enter_staging_area_operation); + if (starting_at_staging_area_) { + OperationID pickup_operation = OperationID::ENTER_STAGING_AREA; + pt.put("operation", pickup_operation); } else { - pt.put("operation", _enter_port_operation); + OperationID enter_port_operation = OperationID::ENTER_PORT; + pt.put("operation", enter_port_operation); } // Add cargo_id if CMV is carrying cargo - if (_cargo_id != "") { - pt.put("cargo_id", _cargo_id); + if (cargo_id_ != "") { + pt.put("cargo_id", cargo_id_); } } // If CMV has arrived at a received destination, add necessary fields based on the destination type that it has arrived at - else if (_pdsm.get_state() == PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION) { + else if (pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION) { // Assign the 'operation' using the 'operation' from the last received port drayage message - pt.put("operation", _latest_mobility_operation_msg.operation); + pt.put("operation", latest_mobility_operation_msg_.operation); // Assign the 'action_id' using the 'action_id' from the last received port drayage message - if (_latest_mobility_operation_msg.current_action_id) { - pt.put("action_id", *_latest_mobility_operation_msg.current_action_id); + if (latest_mobility_operation_msg_.current_action_id) { + pt.put("action_id", *latest_mobility_operation_msg_.current_action_id); } else { - ROS_WARN_STREAM("CMV has arrived at a received destination, but does not have an action_id to broadcast."); + RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at a received destination, but does not have an action_id to broadcast."); } // Assign specific fields for arrival at a Pickup location - if (_latest_mobility_operation_msg.operation == _pickup_operation) { + if (latest_mobility_operation_msg_.operation == OperationID::PICKUP) { // Assign 'cargo_id' using the value received in the previous port drayage message - if (_latest_mobility_operation_msg.cargo_id) { - pt.put("cargo_id", *_latest_mobility_operation_msg.cargo_id); + if (latest_mobility_operation_msg_.cargo_id) { + pt.put("cargo_id", *latest_mobility_operation_msg_.cargo_id); } else { - ROS_WARN_STREAM("CMV has arrived at loading area, but does not have a cargo_id to broadcast."); + RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at loading area, but does not have a cargo_id to broadcast."); } - if (_cargo_id != "") { - ROS_WARN_STREAM("CMV has arrived at a loading area, but it is already carrying cargo."); + if (cargo_id_ != "") { + RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at a loading area, but it is already carrying cargo."); } } // Assign specific fields for arrival at a Dropoff location - else if (_latest_mobility_operation_msg.operation == _dropoff_operation) { + else if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF) { // Assign 'cargo_id' using the ID of the cargo currently being carried - if (_cargo_id != "") { - pt.put("cargo_id", _cargo_id); + if (cargo_id_ != "") { + pt.put("cargo_id", cargo_id_); } else { - ROS_WARN_STREAM("CMV has arrived at a dropoff area, but does not have a cargo_id to broadcast."); + RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at a dropoff area, but does not have a cargo_id to broadcast."); } } // Assign 'cargo_id' using the ID of the cargo currently being carried if the CMV is not arriving at a Pickup location else { - if (_cargo_id != "") { - pt.put("cargo_id", _cargo_id); + if (cargo_id_ != "") { + pt.put("cargo_id", cargo_id_); } } } @@ -252,9 +239,9 @@ namespace port_drayage_plugin return msg; } - void PortDrayageWorker::on_inbound_mobility_operation(const cav_msgs::MobilityOperationConstPtr& msg) { + void PortDrayageWorker::onInboundMobilityOperation(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg) { // Check if the received message is a new message for port drayage - if((msg->strategy == PORT_DRAYAGE_STRATEGY_ID) && (msg->strategy_params != _previous_strategy_params)) { + if((msg->strategy == PORT_DRAYAGE_STRATEGY_ID) && (msg->strategy_params != previous_strategy_params_)) { // Use Boost Property Tree to parse JSON-encoded strategy_params field in MobilityOperations message using boost::property_tree::ptree; ptree pt; @@ -264,45 +251,45 @@ namespace port_drayage_plugin std::string mobility_operation_cmv_id = pt.get("cmv_id"); // Check if the received MobilityOperation message is intended for this vehicle's cmv_id - if(mobility_operation_cmv_id == _cmv_id) { + if(mobility_operation_cmv_id == cmv_id_) { // Since a new message indicates the previous action was completed, update all cargo-related data members based on the previous action that was completed - update_cargo_information_after_action_completion(_latest_mobility_operation_msg); + updateCargoInformationAfterActionCompletion(latest_mobility_operation_msg_); // Store the previously received operation - _previously_completed_operation = _latest_mobility_operation_msg.operation; + previously_completed_operation_ = latest_mobility_operation_msg_.operation; - ROS_DEBUG_STREAM("Processing new port drayage MobilityOperation message for cmv_id " << mobility_operation_cmv_id); - mobility_operation_message_parser(msg->strategy_params); - _previous_strategy_params = msg->strategy_params; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Processing new port drayage MobilityOperation message for cmv_id " << mobility_operation_cmv_id); + mobilityOperationMessageParser(msg->strategy_params); + previous_strategy_params_ = msg->strategy_params; - _pdsm.process_event(PortDrayageEvent::RECEIVED_NEW_DESTINATION); + pdsm_.processEvent(PortDrayageEvent::RECEIVED_NEW_DESTINATION); } else { - ROS_DEBUG_STREAM("Ignoring received port drayage MobilityOperation message intended for cmv_id " << mobility_operation_cmv_id); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Ignoring received port drayage MobilityOperation message intended for cmv_id " << mobility_operation_cmv_id); } } } - void PortDrayageWorker::update_cargo_information_after_action_completion(const PortDrayageMobilityOperationMsg& previous_port_drayage_msg) { - // If the previously received message was for 'Pickup' or 'Dropoff', update this object's _cargo_id member accordingly + void PortDrayageWorker::updateCargoInformationAfterActionCompletion(const PortDrayageMobilityOperationMsg& previous_port_drayage_msg) { + // If the previously received message was for 'Pickup' or 'Dropoff', update this object's cargo_id_ member accordingly // Note: This assumes the previous 'Pickup' or 'Dropoff' action was successful - if (previous_port_drayage_msg.operation == _pickup_operation) { + if (previous_port_drayage_msg.operation == OperationID::PICKUP) { if (previous_port_drayage_msg.cargo_id) { - _cargo_id = *previous_port_drayage_msg.cargo_id; - ROS_DEBUG_STREAM("CMV completed pickup action. CMV is now carrying cargo ID " << _cargo_id); + cargo_id_ = *previous_port_drayage_msg.cargo_id; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV completed pickup action. CMV is now carrying cargo ID " << cargo_id_); } else { - ROS_WARN_STREAM("CMV has completed pickup, but there is no Cargo ID associated with the picked up cargo."); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV has completed pickup, but there is no Cargo ID associated with the picked up cargo."); } } - else if (previous_port_drayage_msg.operation == _dropoff_operation) { - ROS_DEBUG_STREAM("CMV completed dropoff action. CMV is no longer carrying cargo ID " << _cargo_id); - _cargo_id = ""; // Empty string is used when no cargo is being carried + else if (previous_port_drayage_msg.operation == OperationID::DROPOFF) { + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV completed dropoff action. CMV is no longer carrying cargo ID " << cargo_id_); + cargo_id_ = ""; // Empty string is used when no cargo is being carried } } - void PortDrayageWorker::mobility_operation_message_parser(std::string mobility_operation_strategy_params) { + void PortDrayageWorker::mobilityOperationMessageParser(std::string mobility_operation_strategy_params) { // Use Boost Property Tree to parse JSON-encoded strategy_params field in MobilityOperations message using boost::property_tree::ptree; ptree pt; @@ -310,121 +297,121 @@ namespace port_drayage_plugin boost::property_tree::json_parser::read_json(mobility_operation_strategy_params_ss, pt); // Parse 'operation' field and assign the PortDrayageEvent type for this message accordingly - _latest_mobility_operation_msg.operation = pt.get("operation"); - ROS_DEBUG_STREAM("operation: " << _latest_mobility_operation_msg.operation); + latest_mobility_operation_msg_.operation = pt.get("operation"); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "operation: " << latest_mobility_operation_msg_.operation); // If this CMV is commanded to pickup new cargo, check that it isn't already carrying cargo - if (_latest_mobility_operation_msg.operation == _pickup_operation && _cargo_id != "") { - ROS_WARN_STREAM("Received 'PICKUP' operation, but CMV is already carrying cargo."); + if (latest_mobility_operation_msg_.operation == OperationID::PICKUP && cargo_id_ != "") { + RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'PICKUP' operation, but CMV is already carrying cargo."); } // If this CMV is commanded to dropoff cargo, check that it is actually carrying cargo - if (_latest_mobility_operation_msg.operation == _dropoff_operation && _cargo_id == "") { - ROS_WARN_STREAM("Received 'DROPOFF' operation, but CMV isn't currently carrying cargo."); + if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF && cargo_id_ == "") { + RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'DROPOFF' operation, but CMV isn't currently carrying cargo."); } // Parse 'cargo_id' field if it exists in strategy_params if (pt.count("cargo_id") != 0){ - _latest_mobility_operation_msg.cargo_id = pt.get("cargo_id"); - ROS_DEBUG_STREAM("cargo id: " << *_latest_mobility_operation_msg.cargo_id); + latest_mobility_operation_msg_.cargo_id = pt.get("cargo_id"); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "cargo id: " << *latest_mobility_operation_msg_.cargo_id); // Log message if the cargo ID being dropped off does not match the cargo ID currently being carried - if (_latest_mobility_operation_msg.operation == _dropoff_operation && _cargo_id != pt.get("cargo_id")) { - ROS_WARN_STREAM("CMV commanded to dropoff an invalid Cargo ID. Currently carrying " << _cargo_id << ", commanded to dropoff " << pt.get("cargo_id")); + if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF && cargo_id_ != pt.get("cargo_id")) { + RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV commanded to dropoff an invalid Cargo ID. Currently carrying " << cargo_id_ << ", commanded to dropoff " << pt.get("cargo_id")); } } else{ // If this message is for 'PICKUP', then the 'cargo_id' field is required - if(_latest_mobility_operation_msg.operation == _pickup_operation) { - ROS_WARN_STREAM("Received 'PICKUP' operation, but no cargo_id was included."); + if(latest_mobility_operation_msg_.operation == OperationID::PICKUP) { + RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'PICKUP' operation, but no cargo_id was included."); } - else if (_latest_mobility_operation_msg.operation == _dropoff_operation) { - ROS_WARN_STREAM("Received 'DROPOFF' operation, but no cargo_id was included."); + else if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF) { + RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'DROPOFF' operation, but no cargo_id was included."); } - _latest_mobility_operation_msg.cargo_id = boost::optional(); + latest_mobility_operation_msg_.cargo_id = boost::optional(); } // Parse 'action_id' field if it exists in strategy_params if (pt.count("action_id") != 0){ - _latest_mobility_operation_msg.current_action_id = pt.get("action_id"); - ROS_DEBUG_STREAM("action id: " << *_latest_mobility_operation_msg.current_action_id); + latest_mobility_operation_msg_.current_action_id = pt.get("action_id"); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "action id: " << *latest_mobility_operation_msg_.current_action_id); } else{ - _latest_mobility_operation_msg.current_action_id = boost::optional(); + latest_mobility_operation_msg_.current_action_id = boost::optional(); } // Parse starting longitude/latitude fields if 'location' field exists in strategy_params: if (pt.count("location") != 0){ - _latest_mobility_operation_msg.start_longitude = pt.get("location.longitude"); - _latest_mobility_operation_msg.start_latitude = pt.get("location.latitude"); - ROS_DEBUG_STREAM("start long: " << *_latest_mobility_operation_msg.start_longitude); - ROS_DEBUG_STREAM("start lat: " << *_latest_mobility_operation_msg.start_latitude); + latest_mobility_operation_msg_.start_longitude = pt.get("location.longitude"); + latest_mobility_operation_msg_.start_latitude = pt.get("location.latitude"); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "start long: " << *latest_mobility_operation_msg_.start_longitude); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "start lat: " << *latest_mobility_operation_msg_.start_latitude); } else { - _latest_mobility_operation_msg.start_longitude = boost::optional(); - _latest_mobility_operation_msg.start_latitude = boost::optional(); + latest_mobility_operation_msg_.start_longitude = boost::optional(); + latest_mobility_operation_msg_.start_latitude = boost::optional(); } // Parse destination longitude/latitude fields if 'destination' field exists in strategy_params: if (pt.count("destination") != 0) { - _latest_mobility_operation_msg.dest_longitude = pt.get("destination.longitude"); - _latest_mobility_operation_msg.dest_latitude = pt.get("destination.latitude"); - ROS_DEBUG_STREAM("dest long: " << *_latest_mobility_operation_msg.dest_longitude); - ROS_DEBUG_STREAM("dest lat: " << *_latest_mobility_operation_msg.dest_latitude); + latest_mobility_operation_msg_.dest_longitude = pt.get("destination.longitude"); + latest_mobility_operation_msg_.dest_latitude = pt.get("destination.latitude"); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "dest long: " << *latest_mobility_operation_msg_.dest_longitude); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "dest lat: " << *latest_mobility_operation_msg_.dest_latitude); } else { - _latest_mobility_operation_msg.dest_longitude = boost::optional(); - _latest_mobility_operation_msg.dest_latitude = boost::optional(); + latest_mobility_operation_msg_.dest_longitude = boost::optional(); + latest_mobility_operation_msg_.dest_latitude = boost::optional(); } } - void PortDrayageWorker::on_guidance_state(const cav_msgs::GuidanceStateConstPtr& msg) { + void PortDrayageWorker::onGuidanceState(carma_planning_msgs::msg::GuidanceState::UniquePtr msg) { // Drayage operations have started when the CMV has been engaged for the first time - if ((msg->state == cav_msgs::GuidanceState::ENGAGED) && (_pdsm.get_state() == PortDrayageState::INACTIVE) && _enable_port_drayage) { - ROS_DEBUG_STREAM("CMV has been engaged for the first time. Processing DRAYAGE_START event."); - _pdsm.process_event(PortDrayageEvent::DRAYAGE_START); + if ((msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED) && (pdsm_.getState() == PortDrayageState::INACTIVE) && enable_port_drayage_) { + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV has been engaged for the first time. Processing DRAYAGE_START event."); + pdsm_.processEvent(PortDrayageEvent::DRAYAGE_START); } } - void PortDrayageWorker::on_route_event(const cav_msgs::RouteEventConstPtr& msg) { + void PortDrayageWorker::onRouteEvent(carma_planning_msgs::msg::RouteEvent::UniquePtr msg) { // CMV has officially arrived at its destination if the previous route was completed and is no longer active - if (_latest_route_event != nullptr) { - if (_latest_route_event->event == cav_msgs::RouteEvent::ROUTE_COMPLETED && msg->event == cav_msgs::RouteEvent::ROUTE_LOADED) { - if (_pdsm.get_state() == PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION || _pdsm.get_state() == PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION) { - ROS_DEBUG_STREAM("CMV completed its previous route, and the previous route is no longer active."); - ROS_DEBUG_STREAM("Processing ARRIVED_AT_DESTINATION event."); - _pdsm.process_event(PortDrayageEvent::ARRIVED_AT_DESTINATION); + if (latest_route_event_ != nullptr) { + if (latest_route_event_->event == carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED && msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_LOADED) { + if (pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION || pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION) { + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV completed its previous route, and the previous route is no longer active."); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Processing ARRIVED_AT_DESTINATION event."); + pdsm_.processEvent(PortDrayageEvent::ARRIVED_AT_DESTINATION); } } } // Update the latest received route event data member - _latest_route_event = msg; + latest_route_event_ = std::move(msg); } - PortDrayageState PortDrayageWorker::get_port_drayage_state() { - return _pdsm.get_state(); + PortDrayageState PortDrayageWorker::getPortDrayageState() { + return pdsm_.getState(); } - void PortDrayageWorker::on_new_pose(const geometry_msgs::PoseStampedConstPtr& msg) { - if (!_map_projector) { - ROS_DEBUG_STREAM("Ignoring pose message as projection string has not been defined"); + void PortDrayageWorker::onNewPose(geometry_msgs::msg::PoseStamped::UniquePtr msg) { + if (!map_projector_) { + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Ignoring pose message as projection string has not been defined"); return; } // Convert pose message contents to a GPS coordinate - lanelet::GPSPoint coord = _map_projector->reverse( { msg->pose.position.x, msg->pose.position.y, msg->pose.position.z } ); + lanelet::GPSPoint coord = map_projector_->reverse( { msg->pose.position.x, msg->pose.position.y, msg->pose.position.z } ); // Update the locally-stored GPS position of the CMV - _current_gps_position.latitude = coord.lat; - _current_gps_position.longitude = coord.lon; + current_gps_position_.latitude = coord.lat; + current_gps_position_.longitude = coord.lon; } - void PortDrayageWorker::on_new_georeference(const std_msgs::StringConstPtr& msg) { + void PortDrayageWorker::onNewGeoreference(std_msgs::msg::String::UniquePtr msg) { // Build projector from proj string - _map_projector = std::make_shared(msg->data.c_str()); + map_projector_ = std::make_shared(msg->data.c_str()); } -} // namespace port_drayage_plugin +} // namespace port_drayage_plugin \ No newline at end of file diff --git a/port_drayage_plugin/test/test_port_drayage_plugin.cpp b/port_drayage_plugin/test/test_port_drayage_plugin.cpp index 48c46897a8..62cc7281ea 100644 --- a/port_drayage_plugin/test/test_port_drayage_plugin.cpp +++ b/port_drayage_plugin/test/test_port_drayage_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2022 LEIDOS. + * Copyright (C) 2020-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 @@ -14,61 +14,60 @@ * the License. */ -#include "port_drayage_plugin/port_drayage_worker.h" -#include "port_drayage_plugin/port_drayage_state_machine.h" -#include "port_drayage_plugin/port_drayage_plugin.h" #include -#include +#include +#include +#include +#include + +#include "port_drayage_plugin/port_drayage_worker.hpp" +#include "port_drayage_plugin/port_drayage_state_machine.hpp" +#include "port_drayage_plugin/port_drayage_plugin.hpp" #include #include -#include - -#include -#include -#include -#include -#include TEST(PortDrayageTest, testComposeArrivalMessage) { - // Test initial arrival message for pdw initialized with cargo with the Staging Area Entrance as its first destination - ros::Time::init(); - port_drayage_plugin::PortDrayageWorker pdw{ - "DOT-11111", // CMV ID - "321", // Cargo ID - "TEST_CARMA_HOST_ID", // Host ID - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; + auto node = std::make_shared("test_node"); + rclcpp::Clock::SharedPtr clock = node->get_clock(); + + // Test initial arrival message for pdw that has cargo and has the Staging Area Entrance as its first destination + port_drayage_plugin::PortDrayageWorker pdw(node->get_node_logging_interface(), + clock, + [](carma_v2x_msgs::msg::MobilityOperation){}, + [](carma_msgs::msg::UIInstructions){}, + [](std::shared_ptr){return true;} + ); + + pdw.setVehicleID("DOT-11111"); + pdw.setCargoID("321"); + pdw.setEnablePortDrayageFlag(true); + pdw.setStartingAtStagingAreaFlag(true); // Set the pdw's map projector set its current pose std::string base_proj = "+proj=tmerc +lat_0=38.95622708 +lon_0=-77.15066142 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; - std_msgs::String georeference_msg; + std_msgs::msg::String georeference_msg; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr); + std::unique_ptr georeference_msg_ptr = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr)); // State Machine should transition to EN_ROUTE_TO_INITIAL_DESTINATION after guidance state is first engaged - cav_msgs::GuidanceState guidance_state; - guidance_state.state = cav_msgs::GuidanceState::ENGAGED; - cav_msgs::GuidanceStateConstPtr guidance_state_pointer(new cav_msgs::GuidanceState(guidance_state)); - pdw.on_guidance_state(guidance_state_pointer); + carma_planning_msgs::msg::GuidanceState guidance_state; + guidance_state.state = carma_planning_msgs::msg::GuidanceState::ENGAGED; + std::unique_ptr guidance_state_ptr = std::make_unique(guidance_state); + pdw.onGuidanceState(std::move(guidance_state_ptr)); - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr); // Sets the host vehicle's current gps lat/lon position - - cav_msgs::MobilityOperation msg = pdw.compose_arrival_message(); + std::unique_ptr pose_msg_ptr = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr)); // Sets the host vehicle's current gps lat/lon position + carma_v2x_msgs::msg::MobilityOperation msg = pdw.composeArrivalMessage(); + ASSERT_EQ("carma/port_drayage", msg.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg.m_header.sender_id); + ASSERT_EQ("DOT-11111", msg.m_header.sender_id); ASSERT_FALSE(msg.strategy_params.empty()); std::istringstream strstream(msg.strategy_params); @@ -89,27 +88,30 @@ TEST(PortDrayageTest, testComposeArrivalMessage) ASSERT_NEAR(38.95622708, vehicle_latitude, 0.00000001); ASSERT_NEAR(-77.15066142, vehicle_longitude, 0.00000001); - // Test initial arrival message for pdw initialized without cargo with the Port Entrance as its first destination - port_drayage_plugin::PortDrayageWorker pdw2{ - "123", // CMV ID - "", // Cargo ID - "TEST_CARMA_HOST_ID", - false, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; - - pdw2.on_new_georeference(georeference_msg_ptr); - pdw2.on_new_pose(pose_msg_ptr); - pdw2.on_guidance_state(guidance_state_pointer); - - cav_msgs::MobilityOperation msg2 = pdw2.compose_arrival_message(); + // Test initial arrival message for pdw that doesn't have cargo and has the Port Entrance as its first destination + port_drayage_plugin::PortDrayageWorker pdw2(node->get_node_logging_interface(), + clock, + [](carma_v2x_msgs::msg::MobilityOperation){}, + [](carma_msgs::msg::UIInstructions){}, + [](std::shared_ptr){return true;} + ); + + pdw2.setVehicleID("123"); + pdw2.setCargoID(""); + pdw2.setEnablePortDrayageFlag(true); + pdw2.setStartingAtStagingAreaFlag(false); // Sets the Port Entrance as the first destination + + std::unique_ptr georeference_msg_ptr2 = std::make_unique(georeference_msg); + std::unique_ptr pose_msg_ptr2 = std::make_unique(pose_msg); + std::unique_ptr guidance_state_ptr2 = std::make_unique(guidance_state); + pdw2.onNewGeoreference(std::move(georeference_msg_ptr2)); + pdw2.onNewPose(std::move(pose_msg_ptr2)); + pdw2.onGuidanceState(std::move(guidance_state_ptr2)); + + carma_v2x_msgs::msg::MobilityOperation msg2 = pdw2.composeArrivalMessage(); ASSERT_EQ("carma/port_drayage", msg2.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg2.m_header.sender_id); + ASSERT_EQ("123", msg2.m_header.sender_id); ASSERT_FALSE(msg2.strategy_params.empty()); std::istringstream strstream2(msg2.strategy_params); @@ -130,479 +132,227 @@ TEST(PortDrayageTest, testComposeArrivalMessage) ASSERT_NEAR(-77.15066142, vehicle_longitude2, 0.00000001); } -TEST(PortDrayageTest, testCheckStop1) -{ - ros::Time::init(); - port_drayage_plugin::PortDrayageWorker pdw{ - "123", // CMV ID - "321", // Cargo ID - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; - - geometry_msgs::TwistStamped twist; - twist.twist.linear.x = 0; - geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}}; - pdw.set_current_speed(twistptr); - - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver mvr; - mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT; - mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "port_drayage_plugin"; - plan.maneuvers.push_back(mvr); - cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}}; - pdw.set_maneuver_plan(planptr); - - bool stopped = pdw.check_for_stop(planptr, twistptr); - - ASSERT_TRUE(stopped); -} - -TEST(PortDrayageTest, testCheckStop2) -{ - ros::Time::init(); - port_drayage_plugin::PortDrayageWorker pdw{ - "123", // CMV ID - "321", // Cargo ID - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; - - geometry_msgs::TwistStamped twist; - twist.twist.linear.x = 2.0; - geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}}; - pdw.set_current_speed(twistptr); - - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver mvr; - mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT; - mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "Port Drayage Plugin"; - plan.maneuvers.push_back(mvr); - cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}}; - pdw.set_maneuver_plan(planptr); - - bool stopped = pdw.check_for_stop(planptr, twistptr); - - ASSERT_FALSE(stopped); -} - -TEST(PortDrayageTest, testCheckStop3) -{ - ros::Time::init(); - port_drayage_plugin::PortDrayageWorker pdw{ - "123", // CMV ID - "321", // Cargo ID - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; - - geometry_msgs::TwistStamped twist; - twist.twist.linear.x = 0.0; - geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}}; - pdw.set_current_speed(twistptr); - - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver mvr; - mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT; - mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "Route Following Plugin"; - plan.maneuvers.push_back(mvr); - plan.maneuvers.push_back(mvr); - cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}}; - pdw.set_maneuver_plan(planptr); - - bool stopped = pdw.check_for_stop(planptr, twistptr); - - ASSERT_FALSE(stopped); -} - -TEST(PortDrayageTest, testCheckStop4) -{ - ros::Time::init(); - port_drayage_plugin::PortDrayageWorker pdw{ - "123", // CMV ID - "321", // Cargo ID - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; - - geometry_msgs::TwistStamped twist; - twist.twist.linear.x = 2.0; - geometry_msgs::TwistStampedConstPtr twistptr{ new geometry_msgs::TwistStamped{twist}}; - pdw.set_current_speed(twistptr); - - cav_msgs::ManeuverPlan plan; - cav_msgs::Maneuver mvr; - mvr.type = cav_msgs::Maneuver::STOP_AND_WAIT; - mvr.stop_and_wait_maneuver.parameters.planning_strategic_plugin = "Route Following Plugin"; - plan.maneuvers.push_back(mvr); - cav_msgs::ManeuverPlanConstPtr planptr{ new cav_msgs::ManeuverPlan{plan}}; - pdw.set_maneuver_plan(planptr); - - bool stopped = pdw.check_for_stop(planptr, twistptr); - - ASSERT_FALSE(stopped); -} - // Test State Machine flow strictly from PortDrayageEvents TEST(PortDrayageTest, testStateMachine1) { - port_drayage_plugin::PortDrayageStateMachine pdsm; + auto node = std::make_shared("test_node"); + port_drayage_plugin::PortDrayageStateMachine pdsm(node->get_node_logging_interface()); // Verify system initial state - ASSERT_EQ(port_drayage_plugin::PortDrayageState::INACTIVE, pdsm.get_state()); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::INACTIVE, pdsm.getState()); // Startup CARMA system and begin automation - pdsm.process_event(port_drayage_plugin::PortDrayageEvent::DRAYAGE_START); + pdsm.processEvent(port_drayage_plugin::PortDrayageEvent::DRAYAGE_START); // Verify that we are en route to our next destination (port) - ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION, pdsm.get_state()); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION, pdsm.getState()); // Notify state machine we've arrived - pdsm.process_event(port_drayage_plugin::PortDrayageEvent::ARRIVED_AT_DESTINATION); - ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdsm.get_state()); + pdsm.processEvent(port_drayage_plugin::PortDrayageEvent::ARRIVED_AT_DESTINATION); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdsm.getState()); // Notify state machine we've recieved the next destination - pdsm.process_event(port_drayage_plugin::PortDrayageEvent::RECEIVED_NEW_DESTINATION); - ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION, pdsm.get_state()); - - // Rest of the state machine to be implemented and validated in future stories + pdsm.processEvent(port_drayage_plugin::PortDrayageEvent::RECEIVED_NEW_DESTINATION); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION, pdsm.getState()); } // Test communication between PortDrayageWorker and PortDrayageStateMachine for State Machine flow TEST(PortDrayageTest, testPortDrayageStateMachine2) { - // Create PortDrayageWorker object with _cmv_id of "123" and no cargo - //std::function fun = [](cav_msgs::MobilityOperation){return;}; - port_drayage_plugin::PortDrayageWorker pdw{ - "123", // CMV ID - "", // Cargo ID; empty string indicates CMV begins without no cargo - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; + auto node = std::make_shared("test_node"); + rclcpp::Clock::SharedPtr clock = node->get_clock(); + + // Test initial arrival message for pdw that has cargo and has the Staging Area Entrance as its first destination + port_drayage_plugin::PortDrayageWorker pdw(node->get_node_logging_interface(), + clock, + [](carma_v2x_msgs::msg::MobilityOperation){}, + [](carma_msgs::msg::UIInstructions){}, + [](std::shared_ptr){return true;} + ); + + pdw.setVehicleID("123"); + pdw.setCargoID(""); + pdw.setEnablePortDrayageFlag(true); + pdw.setStartingAtStagingAreaFlag(true); // State Machine should begin in INACTIVE state - ASSERT_EQ(port_drayage_plugin::PortDrayageState::INACTIVE, pdw.get_port_drayage_state()); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::INACTIVE, pdw.getPortDrayageState()); // State Machine should transition to EN_ROUTE_TO_INITIAL_DESTINATION after guidance state is first engaged - cav_msgs::GuidanceState guidance_state; - guidance_state.state = cav_msgs::GuidanceState::ENGAGED; - cav_msgs::GuidanceStateConstPtr guidance_state_pointer(new cav_msgs::GuidanceState(guidance_state)); - pdw.on_guidance_state(guidance_state_pointer); + carma_planning_msgs::msg::GuidanceState guidance_state; + guidance_state.state = carma_planning_msgs::msg::GuidanceState::ENGAGED; + std::unique_ptr guidance_state_pointer = std::make_unique(guidance_state); + pdw.onGuidanceState(std::move(guidance_state_pointer)); - ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION, pdw.get_port_drayage_state()); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION, pdw.getPortDrayageState()); // State Machine should transition to AWAITING_DIRECTION if a 'ROUTE_LOADED' event occurs immediately after a 'ROUTE_COMPLETED' event - cav_msgs::RouteEvent route_event_1; - route_event_1.event = cav_msgs::RouteEvent::ROUTE_COMPLETED; - cav_msgs::RouteEventConstPtr route_event_pointer_1(new cav_msgs::RouteEvent(route_event_1)); - pdw.on_route_event(route_event_pointer_1); // PortDrayageWorker receives RouteEvent indicating route has been completed - - cav_msgs::RouteEvent route_event_2; - route_event_2.event = cav_msgs::RouteEvent::ROUTE_LOADED; - cav_msgs::RouteEventConstPtr route_event_pointer_2(new cav_msgs::RouteEvent(route_event_2)); - pdw.on_route_event(route_event_pointer_2); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active + carma_planning_msgs::msg::RouteEvent route_event_1; + route_event_1.event = carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED; + std::unique_ptr route_event_pointer_1 = std::make_unique(route_event_1); + pdw.onRouteEvent(std::move(route_event_pointer_1)); // PortDrayageWorker receives RouteEvent indicating route has been completed + + carma_planning_msgs::msg::RouteEvent route_event_2; + route_event_2.event = carma_planning_msgs::msg::RouteEvent::ROUTE_LOADED; + std::unique_ptr route_event_pointer_2 = std::make_unique(route_event_2); + pdw.onRouteEvent(std::move(route_event_pointer_2)); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active - ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdw.get_port_drayage_state()); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdw.getPortDrayageState()); // State Machine should transition to 'EN_ROUTE_TO_RECEIVED_DESTINATION' if a new port drayage MobilityOperation message is received - // Create a MobilityOperationConstPtr with a cmv_id that is intended for this specific vehicle + // Create a MobilityOperation with a cmv_id that is intended for this specific vehicle // Note: The strategy_params using the schema for messages of this type that have strategy "carma/port_drayage" - cav_msgs::MobilityOperation mobility_operation_msg; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg; mobility_operation_msg.strategy = "carma/port_drayage"; mobility_operation_msg.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"location\"\ : { \"latitude\": \"38.9554377\", \"longitude\": \"-77.1503421\" }, \"destination\": { \"latitude\"\ : \"38.9550038\", \"longitude\": \"-77.1481983\" }, \"operation\": \"PICKUP\", \"action_id\"\ : \"32\"}"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr(new cav_msgs::MobilityOperation(mobility_operation_msg)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr); + std::unique_ptr mobility_operation_msg_ptr = std::make_unique(mobility_operation_msg); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr)); - ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION, pdw.get_port_drayage_state()); + ASSERT_EQ(port_drayage_plugin::PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION, pdw.getPortDrayageState()); // State Machine should transition to 'AWAITING_DIRECTION' again if a ROUTE_COMPLETED event occurs while the vehicle is stopped - pdw.on_route_event(route_event_pointer_1); // PortDrayageWorker receives RouteEvent indicating route has been completed - pdw.on_route_event(route_event_pointer_2); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active - ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdw.get_port_drayage_state()); -} - -TEST(PortDrayageTest, testEstimateDistanceToStop) -{ - ASSERT_EQ(port_drayage_plugin::estimate_distance_to_stop(10.0, 1.0), 50.0); -} - -TEST(PortDrayageTest, testEstimateTimeToStop) -{ - ASSERT_EQ(port_drayage_plugin::estimate_time_to_stop(10.0, 1.0), 20.0); -} - -TEST(PortDrayageTest, testPlanManeuverCb) -{ - cav_srvs::PlanManeuversRequest req; - cav_srvs::PlanManeuversResponse resp; - - port_drayage_plugin::PortDrayagePlugin pdp{nullptr, nullptr}; - pdp.declaration = 1; - - ASSERT_EQ(pdp.plan_maneuver_cb(req,resp),false); - - geometry_msgs::Twist _cur_speed; - _cur_speed.linear.x = 1; - _cur_speed.linear.y = 2; - - pdp._cur_speed = _cur_speed; - - geometry_msgs::Pose pose; - pose.position.x = 0; - pose.position.y = 0; - pose.position.z = 0; - - geometry_msgs::PoseStamped pose_stamped; - - pose_stamped.pose = pose; - - std::shared_ptr curr_pose_ = std::make_shared(pose_stamped); - pdp.curr_pose_ = curr_pose_; - - std::shared_ptr cmw = std::make_shared(); - - auto pl1 = carma_wm::test::getPoint(0, 0, 0); - auto pl2 = carma_wm::test::getPoint(0, 1, 0); - auto pl3 = carma_wm::test::getPoint(0, 2, 0); - auto pr1 = carma_wm::test::getPoint(1, 0, 0); - auto pr2 = carma_wm::test::getPoint(1, 1, 0); - auto pr3 = carma_wm::test::getPoint(1, 2, 0); - - std::vector left_1 = { pl1, pl2 }; - std::vector right_1 = { pr1, pr2 }; - auto ll_1 = carma_wm::test::getLanelet(left_1, right_1, lanelet::AttributeValueString::SolidSolid, lanelet::AttributeValueString::Dashed); - - std::vector left_2 = { pl2, pl3 }; - std::vector right_2 = { pr2, pr3 }; - auto ll_2 = carma_wm::test::getLanelet(left_2, right_2); - - // 1. Confirm all pointers are false (done above) - // Ensure that none of the returned pointers are valid if the map has not been set - ASSERT_FALSE((bool)cmw->getMap()); - ASSERT_FALSE((bool)cmw->getRoute()); - ASSERT_FALSE((bool)cmw->getMapRoutingGraph()); - - - lanelet::Id stop_line_id = lanelet::utils::getId(); - lanelet::LineString3d virtual_stop_line(stop_line_id, {pl2, pr2}); - // Creat passing control line for solid dashed line - std::shared_ptr stop_and_wait(new lanelet::StopRule(lanelet::StopRule::buildData( - lanelet::utils::getId(), { virtual_stop_line }, { lanelet::Participants::Vehicle }))); - - ll_1.addRegulatoryElement(stop_and_wait); - - - // 2. Build map but do not assign - // Create basic map and verify that the map and routing graph can be build, but the route remains false - lanelet::LaneletMapPtr map = lanelet::utils::createMap({ ll_1, ll_2 }, {}); - - // // traffic lights are created from a linestring that shows a traffic light and optionally a stop line. - // lanelet::LineString3d stop_rule = lanelet::LineString3d(lanelet::utils::getId(), {lanelet::Point3d{lanelet::utils::getId(), 1, 0, 0}, lanelet::Point3d{lanelet::utils::getId(), 1, 1, 0}, lanelet::Point3d{lanelet::utils::getId(), 1, 2, 0}}); - // trafficLight.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::TrafficLight; - // // this creates our traffic light. Regelems are passed around as shared pointers. - // lanelet::RegulatoryElementPtr trafficLightRegelem = lanelet::TrafficLight::make(lanelet::utils::getId(), {}, {trafficLight}); - - // // 3. Build routing graph but do not assign - // // Build routing graph from map - lanelet::traffic_rules::TrafficRulesUPtr traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::VehicleCar); - - lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*map, *traffic_rules); - - // 4. Generate route - auto optional_route = map_graph->getRoute(ll_1, ll_2); - ASSERT_TRUE((bool)optional_route); - - lanelet::routing::Route route = std::move(*optional_route); - carma_wm::LaneletRoutePtr route_ptr = std::make_shared(std::move(route)); - // 5. Try to set route without map and ensure it passes - cmw->setRoute(route_ptr); - // 6. getRoute is true but other pointers are false - ASSERT_FALSE((bool)cmw->getMap()); - ASSERT_TRUE((bool)cmw->getRoute()); - ASSERT_FALSE((bool)cmw->getMapRoutingGraph()); - - cmw->setMap(map); - // 8. All pointers exist - ASSERT_TRUE((bool)cmw->getMap()); - ASSERT_TRUE((bool)cmw->getRoute()); - ASSERT_TRUE((bool)cmw->getMapRoutingGraph()); - // 9. Call setRoute again to confirm no errors - cmw->setRoute(route_ptr); - // 10. All pointers exist - ASSERT_TRUE((bool)cmw->getMap()); - ASSERT_TRUE((bool)cmw->getRoute()); - ASSERT_TRUE((bool)cmw->getMapRoutingGraph()); - - pdp.wm_ = cmw; - - ASSERT_EQ(pdp.plan_maneuver_cb(req,resp),true); - - ros::Time time; - time.sec = 1; - time.nsec = 0; - - cav_msgs::Maneuver maneuver = pdp.compose_stop_and_wait_maneuver_message(1, 2, 1, 2, 1, time, 10); - ASSERT_EQ(maneuver.stop_and_wait_maneuver.end_time,time + ros::Duration(15)); - - maneuver = pdp.compose_lane_following_maneuver_message(1, 2, 1, 2, 1, time); - ASSERT_NEAR(maneuver.lane_following_maneuver.end_time.sec, 1, 0.0001); + std::unique_ptr route_event_pointer_3 = std::make_unique(route_event_1); + std::unique_ptr route_event_pointer_4 = std::make_unique(route_event_2); + pdw.onRouteEvent(std::move(route_event_pointer_3)); // PortDrayageWorker receives RouteEvent indicating route has been completed + pdw.onRouteEvent(std::move(route_event_pointer_4)); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active + ASSERT_EQ(port_drayage_plugin::PortDrayageState::AWAITING_DIRECTION, pdw.getPortDrayageState()); } TEST(PortDrayageTest, testComposeSetActiveRouteRequest) { - // Create PortDrayageWorker object with _cmv_id of "123" - port_drayage_plugin::PortDrayageWorker pdw{ - "123", - "TEST_CARGO_ID", - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; - - // Create a MobilityOperationConstPtr with a cmv_id that is intended for this specific vehicle + auto node = std::make_shared("test_node"); + rclcpp::Clock::SharedPtr clock = node->get_clock(); + + // Test initial arrival message for pdw that has cargo and has the Staging Area Entrance as its first destination + port_drayage_plugin::PortDrayageWorker pdw(node->get_node_logging_interface(), + clock, + [](carma_v2x_msgs::msg::MobilityOperation){}, + [](carma_msgs::msg::UIInstructions){}, + [](std::shared_ptr){return true;} + ); + + pdw.setVehicleID("123"); + pdw.setCargoID("TEST_CARGO_ID"); + pdw.setEnablePortDrayageFlag(true); + pdw.setStartingAtStagingAreaFlag(true); + + // Create a MobilityOperation with a cmv_id that is intended for this specific vehicle // Note: The strategy_params using the schema for messages of this type that have strategy "carma/port_drayage" - cav_msgs::MobilityOperation mobility_operation_msg; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg; mobility_operation_msg.strategy = "carma/port_drayage"; mobility_operation_msg.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"cargo\": \"false\", \"location\"\ : { \"latitude\": \"38.9554377\", \"longitude\": \"-77.1503421\" }, \"destination\": { \"latitude\"\ : \"38.9550038\", \"longitude\": \"-77.1481983\" }, \"operation\": \"MOVING_TO_LOADING_AREA\", \"action_id\"\ : \"32\", \"next_action\": \"33\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr(new cav_msgs::MobilityOperation(mobility_operation_msg)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr); - - // Verify the results of PortDrayageWorker's compose_set_active_route_request() method - cav_srvs::SetActiveRoute route_req = pdw.compose_set_active_route_request(*pdw._latest_mobility_operation_msg.dest_latitude, *pdw._latest_mobility_operation_msg.dest_longitude); - ASSERT_EQ(cav_srvs::SetActiveRouteRequest::DESTINATION_POINTS_ARRAY, route_req.request.choice); - ASSERT_EQ("MOVING_TO_LOADING_AREA", route_req.request.route_id); - ASSERT_EQ(1, route_req.request.destination_points.size()); - ASSERT_EQ(38.9550038, route_req.request.destination_points[0].latitude); - ASSERT_EQ(-77.1481983, route_req.request.destination_points[0].longitude); - ASSERT_EQ(false, route_req.request.destination_points[0].elevation_exists); + std::unique_ptr mobility_operation_msg_ptr = std::make_unique(mobility_operation_msg); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr)); + + // Verify the results of PortDrayageWorker's composeSetActiveRouteRequest() method + std::shared_ptr route_req = pdw.composeSetActiveRouteRequest(*pdw.latest_mobility_operation_msg_.dest_latitude, *pdw.latest_mobility_operation_msg_.dest_longitude); + ASSERT_EQ(carma_planning_msgs::srv::SetActiveRoute::Request::DESTINATION_POINTS_ARRAY, route_req->choice); + ASSERT_EQ("MOVING_TO_LOADING_AREA", route_req->route_id); + ASSERT_EQ(1, route_req->destination_points.size()); + ASSERT_EQ(38.9550038, route_req->destination_points[0].latitude); + ASSERT_EQ(-77.1481983, route_req->destination_points[0].longitude); + ASSERT_EQ(false, route_req->destination_points[0].elevation_exists); } // Test Case for testing all potential inbound Port Drayage MobilityOperation messages TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) { - // Create PortDrayageWorker object with cmv_id of "123" that is not carrying cargo - port_drayage_plugin::PortDrayageWorker pdw{ - "123", // CMV ID - "", // Cargo ID; empty string indicates the CMV is not carrying cargo - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; + auto node = std::make_shared("test_node"); + rclcpp::Clock::SharedPtr clock = node->get_clock(); + + // Test initial arrival message for pdw that has cargo and has the Staging Area Entrance as its first destination + port_drayage_plugin::PortDrayageWorker pdw(node->get_node_logging_interface(), + clock, + [](carma_v2x_msgs::msg::MobilityOperation){}, + [](carma_msgs::msg::UIInstructions){}, + [](std::shared_ptr){return true;} + ); + + pdw.setVehicleID("123"); + pdw.setCargoID(""); + pdw.setEnablePortDrayageFlag(true); + pdw.setStartingAtStagingAreaFlag(true); // State Machine should transition to EN_ROUTE_TO_INITIAL_DESTINATION after guidance state is first engaged - cav_msgs::GuidanceState guidance_state; - guidance_state.state = cav_msgs::GuidanceState::ENGAGED; - cav_msgs::GuidanceStateConstPtr guidance_state_pointer(new cav_msgs::GuidanceState(guidance_state)); - pdw.on_guidance_state(guidance_state_pointer); + carma_planning_msgs::msg::GuidanceState guidance_state; + guidance_state.state = carma_planning_msgs::msg::GuidanceState::ENGAGED; + std::unique_ptr guidance_state_pointer = std::make_unique(guidance_state); + pdw.onGuidanceState(std::move(guidance_state_pointer)); // State Machine should transition to AWAITING_DIRECTION if a 'ROUTE_LOADED' event occurs immediately after a 'ROUTE_COMPLETED' event - cav_msgs::RouteEvent route_event_1; - route_event_1.event = cav_msgs::RouteEvent::ROUTE_COMPLETED; - cav_msgs::RouteEventConstPtr route_event_pointer_1(new cav_msgs::RouteEvent(route_event_1)); - pdw.on_route_event(route_event_pointer_1); // PortDrayageWorker receives RouteEvent indicating route has been completed - - cav_msgs::RouteEvent route_event_2; - route_event_2.event = cav_msgs::RouteEvent::ROUTE_LOADED; - cav_msgs::RouteEventConstPtr route_event_pointer_2(new cav_msgs::RouteEvent(route_event_2)); - pdw.on_route_event(route_event_pointer_2); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active + carma_planning_msgs::msg::RouteEvent route_event_1; + route_event_1.event = carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED; + std::unique_ptr route_event_pointer_1 = std::make_unique(route_event_1); + pdw.onRouteEvent(std::move(route_event_pointer_1)); // PortDrayageWorker receives RouteEvent indicating route has been completed + + carma_planning_msgs::msg::RouteEvent route_event_2; + route_event_2.event = carma_planning_msgs::msg::RouteEvent::ROUTE_LOADED; + std::unique_ptr route_event_pointer_2 = std::make_unique(route_event_2); + pdw.onRouteEvent(std::move(route_event_pointer_2)); // PortDrayageWorker receives RouteEvent indicating the previously completed route is no longer active // Create a "PICKUP" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg; mobility_operation_msg.strategy = "carma/port_drayage"; mobility_operation_msg.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"location\"\ : { \"latitude\": \"38.9554377\", \"longitude\": \"-77.1503421\" }, \"destination\": { \"latitude\"\ : \"38.9550038\", \"longitude\": \"-77.1481983\" }, \"operation\": \"PICKUP\", \"action_id\"\ : \"32\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr(new cav_msgs::MobilityOperation(mobility_operation_msg)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr); + std::unique_ptr mobility_operation_msg_ptr = std::make_unique(mobility_operation_msg); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr)); // Check that the received message was parsed and stored correctly - ASSERT_EQ("321", *pdw._latest_mobility_operation_msg.cargo_id); - ASSERT_EQ("PICKUP", pdw._latest_mobility_operation_msg.operation); - ASSERT_NEAR(-77.1503421, *pdw._latest_mobility_operation_msg.start_longitude, 0.00000001); - ASSERT_NEAR(38.9554377, *pdw._latest_mobility_operation_msg.start_latitude, 0.00000001); - ASSERT_NEAR(-77.1481983, *pdw._latest_mobility_operation_msg.dest_longitude, 0.00000001); - ASSERT_NEAR(38.9550038, *pdw._latest_mobility_operation_msg.dest_latitude, 0.00000001); - ASSERT_EQ("32", *pdw._latest_mobility_operation_msg.current_action_id); + ASSERT_EQ("321", *pdw.latest_mobility_operation_msg_.cargo_id); + ASSERT_EQ("PICKUP", pdw.latest_mobility_operation_msg_.operation); + ASSERT_NEAR(-77.1503421, *pdw.latest_mobility_operation_msg_.start_longitude, 0.00000001); + ASSERT_NEAR(38.9554377, *pdw.latest_mobility_operation_msg_.start_latitude, 0.00000001); + ASSERT_NEAR(-77.1481983, *pdw.latest_mobility_operation_msg_.dest_longitude, 0.00000001); + ASSERT_NEAR(38.9550038, *pdw.latest_mobility_operation_msg_.dest_latitude, 0.00000001); + ASSERT_EQ("32", *pdw.latest_mobility_operation_msg_.current_action_id); // Create a MobilityOperationConstPtr with a cmv_id that is not intended for this specific vehicle - cav_msgs::MobilityOperation mobility_operation_msg2; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg2; mobility_operation_msg2.strategy = "carma/port_drayage"; mobility_operation_msg2.strategy_params = "{ \"cmv_id\": \"444\", \"cargo_id\": \"567\", \"location\"\ : { \"latitude\": \"48.9554377\", \"longitude\": \"-67.1503421\" }, \"destination\": { \"latitude\"\ : \"48.9550038\", \"longitude\": \"-57.1481983\" }, \"operation\": \"PICKUP\", \"action_id\"\ : \"44\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr2(new cav_msgs::MobilityOperation(mobility_operation_msg2)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr2); + std::unique_ptr mobility_operation_msg_ptr2 = std::make_unique(mobility_operation_msg2); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr2)); // Check that the contents of the received message was not parsed and stored since it was not intended for this CMV - ASSERT_EQ("321", *pdw._latest_mobility_operation_msg.cargo_id); - ASSERT_EQ("PICKUP", pdw._latest_mobility_operation_msg.operation); - ASSERT_NEAR(-77.1503421, *pdw._latest_mobility_operation_msg.start_longitude, 0.00000001); - ASSERT_NEAR(38.9554377, *pdw._latest_mobility_operation_msg.start_latitude, 0.00000001); - ASSERT_NEAR(-77.1481983, *pdw._latest_mobility_operation_msg.dest_longitude, 0.00000001); - ASSERT_NEAR(38.9550038, *pdw._latest_mobility_operation_msg.dest_latitude, 0.00000001); - ASSERT_EQ("32", *pdw._latest_mobility_operation_msg.current_action_id); + ASSERT_EQ("321", *pdw.latest_mobility_operation_msg_.cargo_id); + ASSERT_EQ("PICKUP", pdw.latest_mobility_operation_msg_.operation); + ASSERT_NEAR(-77.1503421, *pdw.latest_mobility_operation_msg_.start_longitude, 0.00000001); + ASSERT_NEAR(38.9554377, *pdw.latest_mobility_operation_msg_.start_latitude, 0.00000001); + ASSERT_NEAR(-77.1481983, *pdw.latest_mobility_operation_msg_.dest_longitude, 0.00000001); + ASSERT_NEAR(38.9550038, *pdw.latest_mobility_operation_msg_.dest_latitude, 0.00000001); + ASSERT_EQ("32", *pdw.latest_mobility_operation_msg_.current_action_id); // Test composeArrivalMessage for when CMV has arrived at the Loading Area // Set the pdw's map projector and its current pose std::string base_proj = "+proj=tmerc +lat_0=38.95622708 +lon_0=-77.15066142 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; - std_msgs::String georeference_msg; + std_msgs::msg::String georeference_msg; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr); + std::unique_ptr georeference_msg_ptr = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr)); - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Loading Area - cav_msgs::MobilityOperation msg = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg = pdw.composeArrivalMessage(); std::istringstream strstream(msg.strategy_params); using boost::property_tree::ptree; ptree pt; @@ -618,7 +368,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg.m_header.sender_id); + ASSERT_EQ("123", msg.m_header.sender_id); ASSERT_FALSE(msg.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_FALSE(has_cargo); @@ -629,13 +379,13 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.15066142, vehicle_longitude, 0.001); // Create an "EXIT_STAGING_AREA" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg3; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg3; mobility_operation_msg3.strategy = "carma/port_drayage"; mobility_operation_msg3.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\ : \"38.9103493\", \"longitude\": \"-77.1499283\" }, \"operation\": \"EXIT_STAGING_AREA\", \"action_id\"\ : \"34\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr3(new cav_msgs::MobilityOperation(mobility_operation_msg3)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr3); + std::unique_ptr mobility_operation_msg_ptr3 = std::make_unique(mobility_operation_msg3); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr3)); // Test composeArrivalMessage for when CMV has arrived at the Staging Area Exit @@ -643,16 +393,16 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) base_proj = "+proj=tmerc +lat_0=38.9103493 +lon_0=-77.1499283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr2(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr2); + std::unique_ptr georeference_msg_ptr2 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr2)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr2(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr2); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr2 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr2)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Staging Area Exit - cav_msgs::MobilityOperation msg2 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg2 = pdw.composeArrivalMessage(); std::istringstream strstream2(msg2.strategy_params); ptree pt2; boost::property_tree::json_parser::read_json(strstream2, pt2); @@ -667,7 +417,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg2.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg2.m_header.sender_id); + ASSERT_EQ("123", msg2.m_header.sender_id); ASSERT_FALSE(msg2.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_TRUE(has_cargo); @@ -678,13 +428,13 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.1499283, vehicle_longitude, 0.001); // Create an "ENTER_PORT" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg4; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg4; mobility_operation_msg4.strategy = "carma/port_drayage"; mobility_operation_msg4.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\ : \"38.9199993\", \"longitude\": \"-77.1434283\" }, \"operation\": \"ENTER_PORT\", \"action_id\"\ : \"36\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr4(new cav_msgs::MobilityOperation(mobility_operation_msg4)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr4); + std::unique_ptr mobility_operation_msg_ptr4 = std::make_unique(mobility_operation_msg4); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr4)); // Test composeArrivalMessage for when CMV has arrived at the Port Entrance @@ -692,16 +442,16 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) base_proj = "+proj=tmerc +lat_0=38.9199993 +lon_0=-77.1434283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr3(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr3); + std::unique_ptr georeference_msg_ptr3 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr3)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr3(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr3); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr3 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr3)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance - cav_msgs::MobilityOperation msg3 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg3 = pdw.composeArrivalMessage(); std::istringstream strstream3(msg3.strategy_params); ptree pt3; boost::property_tree::json_parser::read_json(strstream3, pt3); @@ -716,7 +466,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg3.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg3.m_header.sender_id); + ASSERT_EQ("123", msg3.m_header.sender_id); ASSERT_FALSE(msg3.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_TRUE(has_cargo); @@ -727,13 +477,13 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.1434283, vehicle_longitude, 0.001); // Create a "DROPOFF" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg5; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg5; mobility_operation_msg5.strategy = "carma/port_drayage"; mobility_operation_msg5.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"321\", \"destination\": { \"latitude\"\ : \"38.34259993\", \"longitude\": \"-77.1224283\" }, \"operation\": \"DROPOFF\", \"action_id\"\ : \"37\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr5(new cav_msgs::MobilityOperation(mobility_operation_msg5)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr5); + std::unique_ptr mobility_operation_msg_ptr5 = std::make_unique(mobility_operation_msg5); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr5)); // Test composeArrivalMessage for when CMV has arrived at the Dropoff location @@ -741,16 +491,16 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) base_proj = "+proj=tmerc +lat_0=38.34259993 +lon_0=-77.1224283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr4(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr4); + std::unique_ptr georeference_msg_ptr4 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr4)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr4(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr4); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr4 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr4)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance - cav_msgs::MobilityOperation msg4 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg4 = pdw.composeArrivalMessage(); std::istringstream strstream4(msg4.strategy_params); ptree pt4; boost::property_tree::json_parser::read_json(strstream4, pt4); @@ -765,7 +515,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg4.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg4.m_header.sender_id); + ASSERT_EQ("123", msg4.m_header.sender_id); ASSERT_FALSE(msg4.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_TRUE(has_cargo); @@ -776,29 +526,29 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.1224283, vehicle_longitude, 0.001); // Create a "PICKUP" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg6; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg6; mobility_operation_msg6.strategy = "carma/port_drayage"; mobility_operation_msg6.strategy_params = "{ \"cmv_id\": \"123\", \"cargo_id\": \"422\", \"destination\": { \"latitude\"\ : \"38.3119993\", \"longitude\": \"-77.2314283\" }, \"operation\": \"PICKUP\", \"action_id\"\ : \"38\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr6(new cav_msgs::MobilityOperation(mobility_operation_msg6)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr6); + std::unique_ptr mobility_operation_msg_ptr6 = std::make_unique(mobility_operation_msg6); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr6)); // Test composeArrivalMessage for when CMV has arrived at the Pickup location // Set the pdw's map projector and its current pose base_proj = "+proj=tmerc +lat_0=38.3119993 +lon_0=-77.2314283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr5(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr5); + std::unique_ptr georeference_msg_ptr5 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr5)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr5(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr5); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr5 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr5)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance - cav_msgs::MobilityOperation msg5 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg5 = pdw.composeArrivalMessage(); std::istringstream strstream5(msg5.strategy_params); ptree pt5; boost::property_tree::json_parser::read_json(strstream5, pt5); @@ -813,7 +563,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg5.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg5.m_header.sender_id); + ASSERT_EQ("123", msg5.m_header.sender_id); ASSERT_FALSE(msg5.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_FALSE(has_cargo); @@ -824,29 +574,29 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.2314283, vehicle_longitude, 0.001); // Create an "PORT_CHECKPOINT" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg7; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg7; mobility_operation_msg7.strategy = "carma/port_drayage"; mobility_operation_msg7.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\ : \"38.3339993\", \"longitude\": \"-77.2594283\" }, \"operation\": \"PORT_CHECKPOINT\", \"action_id\"\ : \"39\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr7(new cav_msgs::MobilityOperation(mobility_operation_msg7)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr7); + std::unique_ptr mobility_operation_msg_ptr7(new carma_v2x_msgs::msg::MobilityOperation(mobility_operation_msg7)); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr7)); // Test composeArrivalMessage for when CMV has arrived at the Port Checkpoint // Set the pdw's map projector and its current pose base_proj = "+proj=tmerc +lat_0=38.3339993 +lon_0=-77.2594283 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr6(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr6); + std::unique_ptr georeference_msg_ptr6 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr6)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr6(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr6); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr6 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr6)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance - cav_msgs::MobilityOperation msg6 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg6 = pdw.composeArrivalMessage(); std::istringstream strstream6(msg6.strategy_params); ptree pt6; boost::property_tree::json_parser::read_json(strstream6, pt6); @@ -861,7 +611,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg6.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg6.m_header.sender_id); + ASSERT_EQ("123", msg6.m_header.sender_id); ASSERT_FALSE(msg6.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_TRUE(has_cargo); @@ -872,29 +622,29 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.2594283, vehicle_longitude, 0.001); // Create a "HOLDING_AREA" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg8; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg8; mobility_operation_msg8.strategy = "carma/port_drayage"; mobility_operation_msg8.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\ : \"38.4139993\", \"longitude\": \"-77.2595583\" }, \"operation\": \"HOLDING_AREA\", \"action_id\"\ : \"40\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr8(new cav_msgs::MobilityOperation(mobility_operation_msg8)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr8); + std::unique_ptr mobility_operation_msg_ptr8 = std::make_unique(mobility_operation_msg8); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr8)); // Test composeArrivalMessage for when CMV has arrived at the Holding Area // Set the pdw's map projector and its current pose base_proj = "+proj=tmerc +lat_0=38.4139993 +lon_0=-77.2595583 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr7(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr7); + std::unique_ptr georeference_msg_ptr7 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr7)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr7(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr7); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr7 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr7)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance - cav_msgs::MobilityOperation msg7 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg7 = pdw.composeArrivalMessage(); std::istringstream strstream7(msg7.strategy_params); ptree pt7; boost::property_tree::json_parser::read_json(strstream7, pt7); @@ -909,7 +659,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg7.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg7.m_header.sender_id); + ASSERT_EQ("123", msg7.m_header.sender_id); ASSERT_FALSE(msg7.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_TRUE(has_cargo); @@ -920,29 +670,29 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.2595583, vehicle_longitude, 0.001); // Create an "EXIT_PORT" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg9; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg9; mobility_operation_msg9.strategy = "carma/port_drayage"; mobility_operation_msg9.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\ : \"38.6639993\", \"longitude\": \"-77.8395583\" }, \"operation\": \"EXIT_PORT\", \"action_id\"\ : \"41\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr9(new cav_msgs::MobilityOperation(mobility_operation_msg9)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr9); + std::unique_ptr mobility_operation_msg_ptr9 = std::make_unique(mobility_operation_msg9); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr9)); // Test composeArrivalMessage for when CMV has arrived at the Port Exit // Set the pdw's map projector and its current pose base_proj = "+proj=tmerc +lat_0=38.6639993 +lon_0=-77.8395583 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr8(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr8); + std::unique_ptr georeference_msg_ptr8 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr8)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr8(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr8); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr8 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr8)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance - cav_msgs::MobilityOperation msg8 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg8 = pdw.composeArrivalMessage(); std::istringstream strstream8(msg8.strategy_params); ptree pt8; boost::property_tree::json_parser::read_json(strstream8, pt8); @@ -957,7 +707,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg8.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg8.m_header.sender_id); + ASSERT_EQ("123", msg8.m_header.sender_id); ASSERT_FALSE(msg8.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_TRUE(has_cargo); @@ -968,29 +718,29 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) ASSERT_NEAR(-77.8395583, vehicle_longitude, 0.001); // Create an "ENTER_STAGING_AREA" MobilityOperationConstPtr for pdw - cav_msgs::MobilityOperation mobility_operation_msg10; + carma_v2x_msgs::msg::MobilityOperation mobility_operation_msg10; mobility_operation_msg10.strategy = "carma/port_drayage"; mobility_operation_msg10.strategy_params = "{ \"cmv_id\": \"123\", \"destination\": { \"latitude\"\ : \"38.6889993\", \"longitude\": \"-77.8124583\" }, \"operation\": \"ENTER_STAGING_AREA\", \"action_id\"\ : \"42\" }"; - cav_msgs::MobilityOperationConstPtr mobility_operation_msg_ptr10(new cav_msgs::MobilityOperation(mobility_operation_msg10)); - pdw.on_inbound_mobility_operation(mobility_operation_msg_ptr10); + std::unique_ptr mobility_operation_msg_ptr10(new carma_v2x_msgs::msg::MobilityOperation(mobility_operation_msg10)); + pdw.onInboundMobilityOperation(std::move(mobility_operation_msg_ptr10)); // Test composeArrivalMessage for when CMV has arrived at the Staging Area Entrance // Set the pdw's map projector and its current pose base_proj = "+proj=tmerc +lat_0=38.6889993 +lon_0=-77.8124583 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " "+no_defs"; georeference_msg.data = base_proj; - std_msgs::StringConstPtr georeference_msg_ptr9(new std_msgs::String(georeference_msg)); - pdw.on_new_georeference(georeference_msg_ptr9); + std::unique_ptr georeference_msg_ptr9 = std::make_unique(georeference_msg); + pdw.onNewGeoreference(std::move(georeference_msg_ptr9)); pose_msg.pose.position.x = 0.0; pose_msg.pose.position.y = 0.0; - geometry_msgs::PoseStampedConstPtr pose_msg_ptr9(new geometry_msgs::PoseStamped(pose_msg)); - pdw.on_new_pose(pose_msg_ptr9); // Sets the host vehicle's current gps lat/lon position + std::unique_ptr pose_msg_ptr9 = std::make_unique(pose_msg); + pdw.onNewPose(std::move(pose_msg_ptr9)); // Sets the host vehicle's current gps lat/lon position // Obtain the contents of the broadcasted message when the CMV arrives at the Port Entrance - cav_msgs::MobilityOperation msg9 = pdw.compose_arrival_message(); + carma_v2x_msgs::msg::MobilityOperation msg9 = pdw.composeArrivalMessage(); std::istringstream strstream9(msg9.strategy_params); ptree pt9; boost::property_tree::json_parser::read_json(strstream9, pt9); @@ -1005,7 +755,7 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) // Verify the contents of the broadcasted message ASSERT_EQ("carma/port_drayage", msg9.strategy); - ASSERT_EQ("TEST_CARMA_HOST_ID", msg9.m_header.sender_id); + ASSERT_EQ("123", msg9.m_header.sender_id); ASSERT_FALSE(msg9.strategy_params.empty()); ASSERT_EQ("123", cmv_id); ASSERT_TRUE(has_cargo); @@ -1018,83 +768,94 @@ TEST(PortDrayageTest, testInboundAndComposedMobilityOperation) TEST(PortDrayageTest, testComposeUIInstructions) { - // Create PortDrayageWorker object with _cmv_id of "123" - port_drayage_plugin::PortDrayageWorker pdw{ - "123", - "", // Empty string indicates CMV is not carrying cargo - "TEST_CARMA_HOST_ID", - true, // Flag indicating CMV's first destination; 'true' is Staging Area Entrance, 'false' is Port Entrance - [](cav_msgs::MobilityOperation){}, - [](cav_msgs::UIInstructions){}, - 1.0, - true, // Flag to enable port drayage operations - [](cav_srvs::SetActiveRoute){return true;} - }; + // Create PortDrayageWorker object with _cmv_id of "123" and no cargo + auto node = std::make_shared("test_node"); + rclcpp::Clock::SharedPtr clock = node->get_clock(); + port_drayage_plugin::PortDrayageWorker pdw(node->get_node_logging_interface(), + clock, + [](carma_v2x_msgs::msg::MobilityOperation){}, + [](carma_msgs::msg::UIInstructions){}, + [](std::shared_ptr){return true;} + ); + + pdw.setVehicleID("123"); + pdw.setCargoID(""); + pdw.setEnablePortDrayageFlag(true); + pdw.setStartingAtStagingAreaFlag(true); // First 'operation' is for 'PICKUP'. Verify the created UI Instructions message: std::string current_operation = "PICKUP"; std::string previous_operation = ""; - cav_msgs::UIInstructions ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation); + carma_msgs::msg::UIInstructions ui_instructions_msg = pdw.composeUIInstructions(current_operation, previous_operation); ASSERT_EQ(ui_instructions_msg.msg, "A new Port Drayage route with operation type 'PICKUP' has been received. " "Select YES to engage the system on the route, or select NO to remain " "disengaged."); - ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED); + ASSERT_EQ(ui_instructions_msg.type, carma_msgs::msg::UIInstructions::ACK_REQUIRED); ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active"); // Second received MobilityOperation message is for a 'DROPOFF' operation. The previous 'PICKUP' operation has been completed. current_operation = "DROPOFF"; previous_operation = "PICKUP"; - ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation); + ui_instructions_msg = pdw.composeUIInstructions(current_operation, previous_operation); ASSERT_EQ(ui_instructions_msg.msg, "The pickup action was completed successfully. A new Port Drayage route with operation type 'DROPOFF' has been received. " "Select YES to engage the system on the route, or select NO to remain " "disengaged."); - ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED); + ASSERT_EQ(ui_instructions_msg.type, carma_msgs::msg::UIInstructions::ACK_REQUIRED); ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active"); // Third received MobilityOperation message is for a 'EXIT_STAGING_AREA' operation. The previous 'DROPOFF' operation has been completed. current_operation = "EXIT_STAGING_AREA"; previous_operation = "DROPOFF"; - ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation); + ui_instructions_msg = pdw.composeUIInstructions(current_operation, previous_operation); ASSERT_EQ(ui_instructions_msg.msg, "The dropoff action was completed successfully. A new Port Drayage route with operation type 'EXIT_STAGING_AREA' has been received. " "Select YES to engage the system on the route, or select NO to remain " "disengaged."); - ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED); + ASSERT_EQ(ui_instructions_msg.type, carma_msgs::msg::UIInstructions::ACK_REQUIRED); ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active"); // Fourth received MobilityOperation message is for a 'ENTER_PORT' operation. The previous 'EXIT_STAGING_AREA' operation has been completed. current_operation = "ENTER_PORT"; previous_operation = "EXIT_STAGING_AREA"; - ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation); + ui_instructions_msg = pdw.composeUIInstructions(current_operation, previous_operation); ASSERT_EQ(ui_instructions_msg.msg, "A new Port Drayage route with operation type 'ENTER_PORT' has been received. " "Select YES to engage the system on the route, or select NO to remain " "disengaged."); - ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED); + ASSERT_EQ(ui_instructions_msg.type, carma_msgs::msg::UIInstructions::ACK_REQUIRED); ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active"); // Fifth received MobilityOperation message is for a 'ENTER_PORT' operation. The previous 'EXIT_STAGING_AREA' operation has been completed. current_operation = "EXIT_PORT"; previous_operation = "HOLDING_AREA"; - ui_instructions_msg = pdw.compose_ui_instructions(current_operation, previous_operation); + ui_instructions_msg = pdw.composeUIInstructions(current_operation, previous_operation); ASSERT_EQ(ui_instructions_msg.msg, "The inspection was completed successfully. A new Port Drayage route with operation type 'EXIT_PORT' has been received. " "Select YES to engage the system on the route, or select NO to remain " "disengaged."); - ASSERT_EQ(ui_instructions_msg.type, cav_msgs::UIInstructions::ACK_REQUIRED); + ASSERT_EQ(ui_instructions_msg.type, carma_msgs::msg::UIInstructions::ACK_REQUIRED); ASSERT_EQ(ui_instructions_msg.response_service, "/guidance/set_guidance_active"); } -// Run all the tests -int main(int argc, char **argv) + +int main(int argc, char ** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} + ::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 From cb4b8d1a4bec1c37aa7104e4c4837629500bc06e Mon Sep 17 00:00:00 2001 From: SaikrishnaBairamoni Date: Fri, 15 Jul 2022 13:00:38 -0400 Subject: [PATCH 034/165] updated config and Dockerfile to point base covrg images --- .circleci/config.yml | 2 +- Dockerfile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 6afaab06f8..f342e69151 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -28,7 +28,7 @@ jobs: # Pull docker image from docker hub # XTERM used for better catkin_make output docker: - - image: usdotfhwastoldev/autoware.ai:develop + - image: usdotfhwastoldev/autoware.ai:coverage user: carma environment: TERM: xterm diff --git a/Dockerfile b/Dockerfile index 3fd6316861..ccb966fa59 100644 --- a/Dockerfile +++ b/Dockerfile @@ -33,7 +33,7 @@ # Stage 1 - Acquire the CARMA source as well as any extra packages # ///////////////////////////////////////////////////////////////////////////// -FROM usdotfhwastoldev/autoware.ai:develop AS base-image +FROM usdotfhwastoldev/autoware.ai:coverage AS base-image FROM base-image AS source-code From 90f5fb70d01f555b84edf8bc73b22c53ab26d6bf Mon Sep 17 00:00:00 2001 From: Aswath <32398753+aswath1@users.noreply.github.com> Date: Tue, 19 Jul 2022 10:23:17 +0530 Subject: [PATCH 035/165] Trajectory Visualizer Port ROS2 (#1844) * trajectory_ros2 * update * updates * warn --- carma/launch/guidance.launch.py | 15 ++- trajectory_visualizer/CMakeLists.txt | 81 +++++-------- .../include/trajectory_visualizer.h | 73 ------------ .../include/trajectory_visualizer.hpp | 72 ++++++++++++ .../include/trajectory_visualizer_config.hpp | 39 +++++++ .../launch/trajectory_visualizer.launch | 18 --- .../launch/trajectory_visualizer_launch.py | 62 ++++++++++ trajectory_visualizer/package.xml | 30 +++-- trajectory_visualizer/src/main.cpp | 26 +++-- .../src/trajectory_visualizer.cpp | 110 ++++++++++-------- 10 files changed, 314 insertions(+), 212 deletions(-) delete mode 100644 trajectory_visualizer/include/trajectory_visualizer.h create mode 100644 trajectory_visualizer/include/trajectory_visualizer.hpp create mode 100644 trajectory_visualizer/include/trajectory_visualizer_config.hpp delete mode 100644 trajectory_visualizer/launch/trajectory_visualizer.launch create mode 100644 trajectory_visualizer/launch/trajectory_visualizer_launch.py diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index 595f6d895a..dd0eebc667 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -229,7 +229,20 @@ def generate_launch_description(): port_drayage_plugin_param_file, vehicle_characteristics_param_file ] - ) + ), + ComposableNode( + package='trajectory_visualizer', + plugin='trajectory_visualizer::TrajectoryVisualizer', + name='trajectory_visualizer_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('trajectory_visualizer', env_log_levels) } + ], + parameters=[ + trajectory_visualizer_param_file + ] + ) + ] ) diff --git a/trajectory_visualizer/CMakeLists.txt b/trajectory_visualizer/CMakeLists.txt index c81fe26611..03567b5770 100644 --- a/trajectory_visualizer/CMakeLists.txt +++ b/trajectory_visualizer/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2018-2021 LEIDOS. +# Copyright (C) 2018-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 @@ -14,71 +14,52 @@ # the License. # -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(trajectory_visualizer) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - autoware_msgs - carma_utils - cav_msgs -) - -## System dependencies are found with CMake's conventions - -################################### -## catkin specific configuration ## -################################### +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp autoware_msgs carma_utils cav_msgs -) - -########### -## Build ## -########### +# Name build targets +set(node_exec trajectory_visualizer_exec) +set(node_lib trajectory_visualizer_lib) +# Includes include_directories( - ${catkin_INCLUDE_DIRS} include ) -add_executable( ${PROJECT_NAME} - src/main.cpp - src/trajectory_visualizer.cpp +ament_auto_add_library(${node_lib} SHARED + src/trajectory_visualizer.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - +ament_auto_add_executable(${node_exec} + src/main.cpp +) -############# -## Install ## -############# +# Register component +rclcpp_components_register_nodes(${node_lib} "trajectory_visualizer::TrajectoryVisualizer") -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -## Install C++ -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# Testing +if(BUILD_TESTING) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -############# -## Testing ## -############# -# no test currently +endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/trajectory_visualizer/include/trajectory_visualizer.h b/trajectory_visualizer/include/trajectory_visualizer.h deleted file mode 100644 index f0a3eac798..0000000000 --- a/trajectory_visualizer/include/trajectory_visualizer.h +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -/* - * Copyright (C) 2020-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 -namespace trajectory_visualizer { - - /** - * TrajectoryVisualizer publishes - * - */ - const double MPH_TO_MS = 0.44704; - - class TrajectoryVisualizer - { - - public: - - /** - * \brief Default constructor - */ - TrajectoryVisualizer(); - - /** - * \brief General starting point to run this node - */ - void run(); - - private: - - // public and private node handles - std::shared_ptr nh_, pnh_; - - // publisher - ros::Publisher traj_marker_pub_; - - // subscriber - ros::Subscriber traj_sub_; - - // initialize this node before running - void initialize(); - - // callbacks - void callbackPlanTrajectory(const cav_msgs::TrajectoryPlan& msg); - - // variables - double max_speed_; - - size_t prev_marker_list_size_ = 0; - // we are not saving every trajectory history at this point - //visualization_msgs::MarkerArray g_global_marker_array; - }; - -} - - diff --git a/trajectory_visualizer/include/trajectory_visualizer.hpp b/trajectory_visualizer/include/trajectory_visualizer.hpp new file mode 100644 index 0000000000..c5bcb138cc --- /dev/null +++ b/trajectory_visualizer/include/trajectory_visualizer.hpp @@ -0,0 +1,72 @@ +#pragma once + +/* + * Copyright (C) 2020-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 "trajectory_visualizer_config.hpp" + +namespace trajectory_visualizer +{ + + /** + * TrajectoryVisualizer . publishes trajectory represented by various colors of rviz markers depending on the speed of trajectory. + * More than 75% of max_speed is red, more 50% is blue, more than 25% is teal, less then 25% is green. + * + */ + const double MPH_TO_MS = 0.44704; + + class TrajectoryVisualizer : public carma_ros2_utils::CarmaLifecycleNode + { + + private: + // Subscribers + carma_ros2_utils::SubPtr traj_sub_; + + // Publishers + carma_ros2_utils::PubPtr traj_marker_pub_; + + // Node configuration + Config config_; + + // callbacks + void callbackPlanTrajectory(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg); + + // variables + double max_speed_; + + size_t prev_marker_list_size_ = 0; + // we are not saving every trajectory history at this point + + public: + + /** + * \brief TrajectoryVisualizer constructor + */ + explicit TrajectoryVisualizer(const rclcpp::NodeOptions &); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + + }; +} + + diff --git a/trajectory_visualizer/include/trajectory_visualizer_config.hpp b/trajectory_visualizer/include/trajectory_visualizer_config.hpp new file mode 100644 index 0000000000..59e327298a --- /dev/null +++ b/trajectory_visualizer/include/trajectory_visualizer_config.hpp @@ -0,0 +1,39 @@ +#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 + +namespace trajectory_visualizer +{ + /** + * \brief max_speed values for trajectory_visualizer + */ + + struct Config + { + double max_speed = 25.0; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "trajectory_visualizer::Config { " << std::endl + << "max_speed: " << c.max_speed << std::endl + << "}" << std::endl; + return output; + } + }; +} // namespace trajectory_visualizer \ No newline at end of file diff --git a/trajectory_visualizer/launch/trajectory_visualizer.launch b/trajectory_visualizer/launch/trajectory_visualizer.launch deleted file mode 100644 index 25787f0da2..0000000000 --- a/trajectory_visualizer/launch/trajectory_visualizer.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - diff --git a/trajectory_visualizer/launch/trajectory_visualizer_launch.py b/trajectory_visualizer/launch/trajectory_visualizer_launch.py new file mode 100644 index 0000000000..e081be91c1 --- /dev/null +++ b/trajectory_visualizer/launch/trajectory_visualizer_launch.py @@ -0,0 +1,62 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA trajectory_visualizer. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='trajectory_visualizer_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='trajectory_visualizer', + plugin='trajectory_visualizer::TrajectoryVisualizer', + name='trajectory_visualizer_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/trajectory_visualizer/package.xml b/trajectory_visualizer/package.xml index 3b3e1af2c9..bcedf7848d 100644 --- a/trajectory_visualizer/package.xml +++ b/trajectory_visualizer/package.xml @@ -1,7 +1,6 @@ - - - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index dd0eebc667..a7eb317e8b 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -72,7 +72,7 @@ def generate_launch_description(): plan_delegator_param_file = os.path.join( get_package_share_directory('plan_delegator'), 'config/plan_delegator_params.yaml') - + port_drayage_plugin_param_file = os.path.join( get_package_share_directory('port_drayage_plugin'), 'config/parameters.yaml') diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index a82ac53c19..9ee9327390 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -47,6 +47,9 @@ def generate_launch_description(): inlanecruising_plugin_file_path = os.path.join( get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml') + stop_and_wait_plugin_param_file = os.path.join( + get_package_share_directory('stop_and_wait_plugin'), 'config/stop_and_wait_plugin_params.yaml') + env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') carma_plugins_container = ComposableNodeContainer( @@ -75,6 +78,27 @@ def generate_launch_description(): inlanecruising_plugin_file_path, vehicle_config_param_file ] + ), + ComposableNode( + package='stop_and_wait_plugin', + plugin='stop_and_wait_plugin::StopandWaitNode', + name='stop_and_wait_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('stop_and_wait_plugin', env_log_levels) } + ], + 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" ] ), + ("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" ] ), + ], + parameters=[ + stop_and_wait_plugin_param_file, + vehicle_config_param_file + ] ) ] ) diff --git a/stop_and_wait_plugin/CMakeLists.txt b/stop_and_wait_plugin/CMakeLists.txt index 5a8003a0e3..835e74a1d9 100644 --- a/stop_and_wait_plugin/CMakeLists.txt +++ b/stop_and_wait_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2018-2020 LEIDOS. +# 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 @@ -13,97 +13,66 @@ # License for the specific language governing permissions and limitations under # the License. # -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(stop_and_wait_plugin) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -set( CATKIN_DEPS - roscpp - std_msgs - cav_msgs - cav_srvs - carma_utils - trajectory_utils - autoware_msgs - carma_wm - tf - tf2 - tf2_geometry_msgs - roslib - basic_autonomy -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) -## System dependencies are found with CMake's conventions -find_package(Boost) -find_package(Eigen3 REQUIRED) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -################################### -## catkin specific configuration ## -################################### - -catkin_package( - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost EIGEN3 -) - -########### -## Build ## -########### +# Name build targets +set(node_exec stop_and_wait_plugin_exec) +set(worker_lib stop_and_wait_plugin_lib) +set(node_lib stop_and_wait_plugin_node) include_directories( include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} ) -file(GLOB_RECURSE headers */*.hpp */*.h) - -add_executable( ${PROJECT_NAME} - ${headers} - src/main.cpp - src/stop_and_wait_plugin.cpp) -add_library(stopandwait_plugin_lib src/stop_and_wait_plugin.cpp) -add_dependencies(stopandwait_plugin_lib ${catkin_EXPORTED_TARGETS}) +# Build +ament_auto_add_library(${worker_lib} SHARED + src/stop_and_wait_plugin.cpp +) -target_link_libraries(stopandwait_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} stopandwait_plugin_lib) +ament_auto_add_library(${node_lib} SHARED + src/stop_and_wait_plugin_node.cpp +) +ament_auto_add_executable(${node_exec} + src/main.cpp +) -############# -## Install ## -############# +# Register component +rclcpp_components_register_nodes(${worker_lib} "stop_and_wait_plugin::StopandWaitNode") -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +# 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} ) -## Install C++ -install(TARGETS ${PROJECT_NAME} stopandwait_plugin_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +target_link_libraries(${node_exec} + ${node_lib} ) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +# Testing +if(BUILD_TESTING) + +find_package(ament_lint_auto REQUIRED) +ament_lint_auto_find_test_dependencies() -############# -## Testing ## -############# +ament_add_gtest(test_stopandwait_plugin test/test_stopandwait_plugin.cpp) +ament_target_dependencies(test_stopandwait_plugin +${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) +target_link_libraries(test_stopandwait_plugin ${worker_lib}) +endif() -catkin_add_gmock(${PROJECT_NAME}-test - test/test_stopandwait_plugin.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) -target_link_libraries(${PROJECT_NAME}-test stopandwait_plugin_lib ${catkin_LIBRARIES}) + # Install +ament_auto_package( + INSTALL_TO_SHARE launch config +) diff --git a/stop_and_wait_plugin/config/parameters.yaml b/stop_and_wait_plugin/config/parameters.yaml index bac4b83a33..aec0de7a1e 100644 --- a/stop_and_wait_plugin/config/parameters.yaml +++ b/stop_and_wait_plugin/config/parameters.yaml @@ -20,7 +20,7 @@ crawl_speed : 0.91 # Double: The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions # Units: m -cernterline_sampling_spacing: 1.0 +centerline_sampling_spacing: 1.0 # Double: The default buffer in meters used for where the vehicle can come to a stop during execution. diff --git a/stop_and_wait_plugin/include/stop_and_wait_config.h b/stop_and_wait_plugin/include/stop_and_wait_config.hpp similarity index 89% rename from stop_and_wait_plugin/include/stop_and_wait_config.h rename to stop_and_wait_plugin/include/stop_and_wait_config.hpp index b70b1665a0..0063bd62d9 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_config.h +++ b/stop_and_wait_plugin/include/stop_and_wait_config.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2021 LEIDOS. + * Copyright (C) 2021-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 @@ -29,7 +29,7 @@ struct StopandWaitConfig double accel_limit_multiplier = 0.5; // Multiplier to compine with actual accel limit for target planning double accel_limit = 2.0; // Longitudinal acceleration limit of the vehicle double crawl_speed = 1.34112; // Minimum speed the vehicle can command before being ready to stop - double cernterline_sampling_spacing = 1.0; // The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions + double centerline_sampling_spacing = 1.0; // The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions double default_stopping_buffer = 3.0; // The default buffer in meters used for where the vehicle can come to a stop during execution. This value is overriden by the first value in maneuver.parameters.float_valued_meta_data[] double moving_average_window_size = 11.0; // Moving Average filter window size friend std::ostream& operator<<(std::ostream& output, const StopandWaitConfig& c) @@ -41,7 +41,7 @@ struct StopandWaitConfig << "accel_limit_multiplier: " << c.accel_limit_multiplier << std::endl << "accel_limit: " << c.accel_limit << std::endl << "crawl_speed: " << c.crawl_speed << std::endl - << "cernterline_sampling_spacing: " << c.crawl_speed << std::endl + << "centerline_sampling_spacing: " << c.crawl_speed << std::endl << "default_stopping_buffer: " << c.crawl_speed << std::endl << "}" << std::endl; return output; diff --git a/stop_and_wait_plugin/include/stop_and_wait_node.h b/stop_and_wait_plugin/include/stop_and_wait_node.h deleted file mode 100644 index 048c36b7ec..0000000000 --- a/stop_and_wait_plugin/include/stop_and_wait_node.h +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once - -/* - * Copyright (C) 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "stop_and_wait_plugin.h" -#include "stop_and_wait_config.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace stop_and_wait_plugin -{ -class StopandWaitNode -{ -public: - /** - * \brief General entry point to begin the operation of this class - */ - void run() - { - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - carma_wm::WMListener wml; - auto wm = wml.getWorldModel(); - - StopandWaitConfig config; - - pnh.param("minimal_trajectory_duration", config.minimal_trajectory_duration, - config.minimal_trajectory_duration); - pnh.param("stop_timestep", config.stop_timestep, config.stop_timestep); - pnh.param("trajectory_step_size", config.trajectory_step_size, config.trajectory_step_size); - pnh.param("accel_limit_multiplier", config.accel_limit_multiplier, config.accel_limit_multiplier); - pnh.param("/vehicle_acceleration_limit", config.accel_limit, config.accel_limit); - pnh.param("crawl_speed", config.crawl_speed, config.crawl_speed); - pnh.param("cernterline_sampling_spacing", config.cernterline_sampling_spacing, config.cernterline_sampling_spacing); - pnh.param("default_stopping_buffer", config.default_stopping_buffer, config.default_stopping_buffer); - - ros::Publisher plugin_discovery_pub = nh.advertise("plugin_discovery", 1); - - StopandWait plugin(wm, config, [&plugin_discovery_pub](auto msg) { plugin_discovery_pub.publish(msg); }); - - ros::ServiceServer trajectory_srv_ = - nh.advertiseService("plan_trajectory", &StopandWait::plan_trajectory_cb, &plugin); - - ros::Timer discovery_pub_timer = - pnh.createTimer(ros::Duration(ros::Rate(10.0)), [&plugin](const auto&) { plugin.spinCallback(); }); - - ros::CARMANodeHandle::spin(); - } -}; -} // namespace stop_and_wait_plugin \ No newline at end of file diff --git a/stop_and_wait_plugin/include/stop_and_wait_node.hpp b/stop_and_wait_plugin/include/stop_and_wait_node.hpp new file mode 100644 index 0000000000..8dc0803132 --- /dev/null +++ b/stop_and_wait_plugin/include/stop_and_wait_node.hpp @@ -0,0 +1,91 @@ +#pragma once + +/* + * Copyright (C) 2021-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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "stop_and_wait_plugin.hpp" +#include "stop_and_wait_config.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace stop_and_wait_plugin +{ +class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin +{ + +private: + + StopandWaitConfig config_; + + // Worker + std::shared_ptr plugin_; + + std::string version_id_; + std::string plugin_name_; + carma_wm::WorldModelConstPtr wm_; + +public: + + /** + * \brief Node constructor + */ + explicit StopandWaitNode(const rclcpp::NodeOptions &); + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + + //// + // Overrides + //// + + void plan_trajectory_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; + + bool get_availability() override; + + std::string get_version_id() override final; + +}; +} // namespace stop_and_wait_plugin \ No newline at end of file diff --git a/stop_and_wait_plugin/include/stop_and_wait_plugin.h b/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp similarity index 65% rename from stop_and_wait_plugin/include/stop_and_wait_plugin.h rename to stop_and_wait_plugin/include/stop_and_wait_plugin.hpp index ea99a8b772..5dae3d2b17 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_plugin.h +++ b/stop_and_wait_plugin/include/stop_and_wait_plugin.hpp @@ -1,6 +1,6 @@ #pragma once /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -16,29 +16,28 @@ */ #include -#include -#include -#include +#include +#include +#include +#include "stop_and_wait_config.hpp" +#include "basic_autonomy_ros2/basic_autonomy.hpp" +#include "basic_autonomy_ros2/helper_functions.hpp" #include -#include -#include -#include -#include -#include +#include +#include #include -#include #include #include -#include -#include -#include -#include "stop_and_wait_config.h" -#include -#include +#include +#include +#include +#include +#include +#include +#include namespace stop_and_wait_plugin { -using PublishPluginDiscoveryCB = std::function; /** * \brief Convenience class for pairing 2d points with speeds */ @@ -54,8 +53,11 @@ class StopandWait /** * \brief Constructor */ - StopandWait(carma_wm::WorldModelConstPtr wm, StopandWaitConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher); + StopandWait(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const StopandWaitConfig& config, + const std::string& plugin_name, + const std::string& version_id); /** * \brief Service callback for trajectory planning @@ -65,17 +67,7 @@ class StopandWait * * \return True if success. False otherwise */ - bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp); - - /** - * \brief Method meant to be called periodically to trigger plugin discovery behavior - */ - bool spinCallback(); - - /** - * \brief General entry point to begin the operation of this class - */ - void run(); + bool plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); /** * \brief Converts a set of requested STOP_AND_WAIT maneuvers to point speed limit pairs. @@ -90,9 +82,9 @@ class StopandWait * * \return List of centerline points paired with speed limits. All output points will have speed matching state.logitudinal_velocity */ - std::vector maneuvers_to_points(const std::vector& maneuvers, + std::vector maneuvers_to_points(const std::vector& maneuvers, const carma_wm::WorldModelConstPtr& wm, - const cav_msgs::VehicleState& state); + const carma_planning_msgs::msg::VehicleState& state); /** * \brief Method converts a list of lanelet centerline points and current vehicle state into a usable list of * trajectory points for trajectory planning @@ -103,9 +95,9 @@ class StopandWait * \param initial_speed Returns the initial_speed used to generate the trajectory * \return A list of trajectory points to send to the carma planning stack */ - std::vector compose_trajectory_from_centerline( + std::vector compose_trajectory_from_centerline( const std::vector& points, double starting_downtrack, double starting_speed, double stop_location, - double stop_location_buffer, ros::Time start_time, double stopping_acceleration, double& initial_speed); + double stop_location_buffer, rclcpp::Time start_time, double stopping_acceleration, double& initial_speed); /** * \brief Helper method to split a list of PointSpeedPair into separate point and speed lists @@ -113,20 +105,20 @@ class StopandWait void splitPointSpeedPairs(const std::vector& points, std::vector* basic_points, std::vector* speeds) const; - std::vector trajectory_from_points_times_orientations( + std::vector trajectory_from_points_times_orientations( const std::vector& points, const std::vector& times, - const std::vector& yaws, ros::Time startTime); + const std::vector& yaws, rclcpp::Time startTime); private: double epsilon_ = 0.001; //small constant to compare double // pointer to the actual wm object + std::string plugin_name_; + std::string version_id_; carma_wm::WorldModelConstPtr wm_; StopandWaitConfig config_; - PublishPluginDiscoveryCB plugin_discovery_publisher_; - - cav_msgs::Plugin plugin_discovery_msg_; + std::shared_ptr nh_; }; } // namespace stop_and_wait_plugin diff --git a/stop_and_wait_plugin/launch/stop_and_wait_plugin.launch b/stop_and_wait_plugin/launch/stop_and_wait_plugin.launch deleted file mode 100644 index f9538ca300..0000000000 --- a/stop_and_wait_plugin/launch/stop_and_wait_plugin.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/stop_and_wait_plugin/launch/stop_and_wait_plugin_launch.py b/stop_and_wait_plugin/launch/stop_and_wait_plugin_launch.py new file mode 100644 index 0000000000..7a5a8dbb4d --- /dev/null +++ b/stop_and_wait_plugin/launch/stop_and_wait_plugin_launch.py @@ -0,0 +1,62 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the stop_and_wait_plugin. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='stop_and_wait_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='stop_and_wait_plugin', + plugin='stop_and_wait_plugin::StopandWaitNode', + name='stop_and_wait_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/stop_and_wait_plugin/package.xml b/stop_and_wait_plugin/package.xml index 050c5ba5b9..2b57376224 100644 --- a/stop_and_wait_plugin/package.xml +++ b/stop_and_wait_plugin/package.xml @@ -1,7 +1,7 @@ stop_and_wait_plugin - 3.3.0 + 4.0.0 The stop_and_wait_plugin package carma Apache License 2.0 carma - catkin + + ament_cmake carma_cmake_common - roscpp + ament_auto_cmake + rclcpp + carma_ros2_utils + rclcpp_components std_msgs - cav_srvs - cav_msgs - carma_utils autoware_msgs - carma_wm - tf + carma_wm_ros2 tf2 tf2_geometry_msgs - trajectory_utils + trajectory_utils_ros2 roslib - basic_autonomy + basic_autonomy_ros2 + carma_guidance_plugins + carma_planning_msgs + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + diff --git a/stop_and_wait_plugin/src/main.cpp b/stop_and_wait_plugin/src/main.cpp index bc92fb97cf..48d7820105 100644 --- a/stop_and_wait_plugin/src/main.cpp +++ b/stop_and_wait_plugin/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -14,14 +14,19 @@ * the License. */ -#include +#include +#include "stop_and_wait_node.hpp" -#include - -int main(int argc, char** argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "stop_and_wait_plugin"); - stop_and_wait_plugin::StopandWaitNode sw; - sw.run(); + 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; } \ No newline at end of file diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp index ca5786339b..3baa7e30c7 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -14,33 +14,31 @@ * the License. */ -#include +#include #include #include #include #include #include #include -#include -#include +#include +#include #include -#include +#include #include #include #include #include #include -#include "stop_and_wait_plugin.h" +#include "stop_and_wait_plugin.hpp" #include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include +#include #include #include #include @@ -49,79 +47,69 @@ using oss = std::ostringstream; namespace stop_and_wait_plugin { -StopandWait::StopandWait(carma_wm::WorldModelConstPtr wm, StopandWaitConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher) - : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher) -{ - plugin_discovery_msg_.name = "stop_and_wait_plugin"; - plugin_discovery_msg_.version_id = "v1.1"; - plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = true; - plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; - plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; -}; - -bool StopandWait::spinCallback() -{ - plugin_discovery_publisher_(plugin_discovery_msg_); - return true; -} -bool StopandWait::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp) +StopandWait::StopandWait(std::shared_ptr nh, + carma_wm::WorldModelConstPtr wm, + const StopandWaitConfig& config, + const std::string& plugin_name, + const std::string& version_id) + : version_id_ (version_id),plugin_name_(plugin_name),config_(config),nh_(nh), wm_(wm) +{}; + +bool StopandWait::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { - ROS_DEBUG_STREAM("Starting stop&wait planning"); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Starting stop&wait planning"); - if (req.maneuver_index_to_plan >= req.maneuver_plan.maneuvers.size()) + if (req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size()) { throw std::invalid_argument( - "StopAndWait plugin asked to plan invalid maneuver index: " + std::to_string(req.maneuver_index_to_plan) + - " for plan of size: " + std::to_string(req.maneuver_plan.maneuvers.size())); + "StopAndWait plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); } - if (req.maneuver_plan.maneuvers[req.maneuver_index_to_plan].type != cav_msgs::Maneuver::STOP_AND_WAIT) + if (req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type != carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT) { throw std::invalid_argument("StopAndWait plugin asked to plan non STOP_AND_WAIT maneuver"); } - lanelet::BasicPoint2d veh_pos(req.vehicle_state.x_pos_global, req.vehicle_state.y_pos_global); + lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global); - ROS_DEBUG_STREAM("planning state x:" << req.vehicle_state.x_pos_global << ", y: " << req.vehicle_state.y_pos_global << ", speed: " << req.vehicle_state.longitudinal_vel); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"planning state x:" << req->vehicle_state.x_pos_global << ", y: " << req->vehicle_state.y_pos_global << ", speed: " << req->vehicle_state.longitudinal_vel); - if (req.vehicle_state.longitudinal_vel < epsilon_) + if (req->vehicle_state.longitudinal_vel < epsilon_) { - ROS_WARN_STREAM("Detected that car is already stopped! Ignoring the request to plan Stop&Wait"); - ROS_DEBUG_STREAM("Detected that car is already stopped! Ignoring the request to plan Stop&Wait"); - + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Detected that car is already stopped! Ignoring the request to plan Stop&Wait"); + return true; } double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack; - ROS_DEBUG_STREAM("Current_downtrack" << current_downtrack); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Current_downtrack" << current_downtrack); - if (req.maneuver_plan.maneuvers[req.maneuver_index_to_plan].stop_and_wait_maneuver.end_dist < current_downtrack) + if (req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].stop_and_wait_maneuver.end_dist < current_downtrack) { throw std::invalid_argument("StopAndWait plugin asked to plan maneuver that ends earlier than the current state."); } - resp.related_maneuvers.push_back(req.maneuver_index_to_plan); - resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); + resp->related_maneuvers.push_back(req->maneuver_index_to_plan); + resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); - std::string maneuver_id = req.maneuver_plan.maneuvers[req.maneuver_index_to_plan].stop_and_wait_maneuver.parameters.maneuver_id; + std::string maneuver_id = req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].stop_and_wait_maneuver.parameters.maneuver_id; - ROS_INFO_STREAM("Maneuver not yet planned, planning new trajectory"); + RCLCPP_INFO_STREAM(nh_->get_logger(),"Maneuver not yet planned, planning new trajectory"); // Maneuver input is valid so continue with execution - std::vector maneuver_plan = { req.maneuver_plan.maneuvers[req.maneuver_index_to_plan] }; + std::vector maneuver_plan = { req->maneuver_plan.maneuvers[req->maneuver_index_to_plan] }; std::vector points_and_target_speeds = maneuvers_to_points( - maneuver_plan, wm_, req.vehicle_state); // Now have 1m downsampled points from cur to endpoint + maneuver_plan, wm_, req->vehicle_state); // Now have 1m downsampled points from cur to endpoint // Trajectory plan - cav_msgs::TrajectoryPlan trajectory; + carma_planning_msgs::msg::TrajectoryPlan trajectory; trajectory.header.frame_id = "map"; - trajectory.header.stamp = req.header.stamp; + trajectory.header.stamp = req->header.stamp; trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); // Extract the stopping buffer used to consider a stopping behavior complete @@ -129,7 +117,7 @@ bool StopandWait::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_s double stopping_accel = 0.0; if (maneuver_plan[0].stop_and_wait_maneuver.parameters.presence_vector & - cav_msgs::ManeuverParameters::HAS_FLOAT_META_DATA) + carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA) { if(maneuver_plan[0].stop_and_wait_maneuver.parameters.float_valued_meta_data.size() < 2){ throw std::invalid_argument("stop and wait maneuver message missing required meta data"); @@ -137,37 +125,37 @@ bool StopandWait::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_s stop_location_buffer = maneuver_plan[0].stop_and_wait_maneuver.parameters.float_valued_meta_data[0]; stopping_accel = maneuver_plan[0].stop_and_wait_maneuver.parameters.float_valued_meta_data[1]; - ROS_DEBUG_STREAM("Using stop buffer from meta data: " << stop_location_buffer); - ROS_DEBUG_STREAM("Using stopping acceleration from meta data: "<< stopping_accel); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Using stop buffer from meta data: " << stop_location_buffer); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Using stopping acceleration from meta data: "<< stopping_accel); } else{ throw std::invalid_argument("stop and wait maneuver message missing required float meta data"); } - double initial_speed = req.vehicle_state.longitudinal_vel; //will be modified after compose_trajectory_from_centerline + double initial_speed = req->vehicle_state.longitudinal_vel; //will be modified after compose_trajectory_from_centerline trajectory.trajectory_points = compose_trajectory_from_centerline( - points_and_target_speeds, current_downtrack, req.vehicle_state.longitudinal_vel, - maneuver_plan[0].stop_and_wait_maneuver.end_dist, stop_location_buffer, req.header.stamp, stopping_accel, initial_speed); + points_and_target_speeds, current_downtrack, req->vehicle_state.longitudinal_vel, + maneuver_plan[0].stop_and_wait_maneuver.end_dist, stop_location_buffer, req->header.stamp, stopping_accel, initial_speed); - ROS_DEBUG_STREAM("Trajectory points size:" << trajectory.trajectory_points.size()); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Trajectory points size:" << trajectory.trajectory_points.size()); trajectory.initial_longitudinal_velocity = initial_speed; - resp.trajectory_plan = trajectory; + resp->trajectory_plan = trajectory; return true; } // Returns the centerline points and speed limits for the provided maneuver -std::vector StopandWait::maneuvers_to_points(const std::vector& maneuvers, +std::vector StopandWait::maneuvers_to_points(const std::vector& maneuvers, const carma_wm::WorldModelConstPtr& wm, - const cav_msgs::VehicleState& state) + const carma_planning_msgs::msg::VehicleState& state) { std::vector points_and_target_speeds; std::unordered_set visited_lanelets; - cav_msgs::StopAndWaitManeuver stop_and_wait_maneuver = maneuvers[0].stop_and_wait_maneuver; + carma_planning_msgs::msg::StopAndWaitManeuver stop_and_wait_maneuver = maneuvers[0].stop_and_wait_maneuver; lanelet::BasicPoint2d veh_pos(state.x_pos_global, state.y_pos_global); double starting_downtrack = wm_->routeTrackPos(veh_pos).downtrack; // The vehicle position @@ -177,8 +165,8 @@ std::vector StopandWait::maneuvers_to_points(const std::vector route_points = wm->sampleRoutePoints( - std::min(starting_downtrack + config_.cernterline_sampling_spacing, stop_and_wait_maneuver.end_dist), - stop_and_wait_maneuver.end_dist, config_.cernterline_sampling_spacing); + std::min(starting_downtrack + config_.centerline_sampling_spacing, stop_and_wait_maneuver.end_dist), + stop_and_wait_maneuver.end_dist, config_.centerline_sampling_spacing); route_points.insert(route_points.begin(), veh_pos); @@ -195,44 +183,43 @@ std::vector StopandWait::maneuvers_to_points(const std::vector StopandWait::trajectory_from_points_times_orientations( +std::vector StopandWait::trajectory_from_points_times_orientations( const std::vector& points, const std::vector& times, const std::vector& yaws, - ros::Time startTime) + rclcpp::Time startTime) { if (points.size() != times.size() || points.size() != yaws.size()) { throw std::invalid_argument("All input vectors must have the same size"); } - std::vector traj; + std::vector traj; traj.reserve(points.size()); for (size_t i = 0; i < points.size(); i++) { - cav_msgs::TrajectoryPlanPoint tpp; - ros::Duration relative_time(times[i]); + carma_planning_msgs::msg::TrajectoryPlanPoint tpp; + rclcpp::Duration relative_time(times[i] * 1e9); tpp.target_time = startTime + relative_time; tpp.x = points[i].x(); tpp.y = points[i].y(); tpp.yaw = yaws[i]; tpp.controller_plugin_name = "default"; - tpp.planner_plugin_name = plugin_discovery_msg_.name; - + traj.push_back(tpp); } return traj; } -std::vector StopandWait::compose_trajectory_from_centerline( +std::vector StopandWait::compose_trajectory_from_centerline( const std::vector& points, double starting_downtrack, double starting_speed, double stop_location, - double stop_location_buffer, ros::Time start_time, double stopping_acceleration, double& initial_speed) + double stop_location_buffer, rclcpp::Time start_time, double stopping_acceleration, double& initial_speed) { - std::vector plan; + std::vector plan; if (points.size() == 0) { - ROS_WARN_STREAM("No points to use as trajectory in stop and wait plugin"); + RCLCPP_WARN_STREAM(nh_->get_logger(),"No points to use as trajectory in stop and wait plugin"); return plan; } @@ -262,11 +249,11 @@ std::vector StopandWait::compose_trajectory_from_ if (req_dist > remaining_distance) { - ROS_DEBUG_STREAM("Target Accel Update From: " << target_accel); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Target Accel Update From: " << target_accel); target_accel = (starting_speed * starting_speed) / (2.0 * remaining_distance); // If we cannot reach the end point it the // required distance update the accel target - ROS_DEBUG_STREAM("Target Accel Update To: " << target_accel); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"Target Accel Update To: " << target_accel); } @@ -283,8 +270,6 @@ std::vector StopandWait::compose_trajectory_from_ for (int i = points.size() - 2; i >= 0; i--) { // NOTE: Do not use size_t for i type here as -- with > 0 will result in overflow - - double v_i = prev_pair.speed; double dx = lanelet::geometry::distance2d(prev_pair.point, points[i].point); double new_downtrack = inverse_downtracks.back() + dx; @@ -309,8 +294,7 @@ std::vector StopandWait::compose_trajectory_from_ prev_pair = pair; continue; // continue until loop end } - - + double v_f = sqrt(v_i * v_i + 2 * target_accel * dx); PointSpeedPair pair = points[i]; @@ -373,7 +357,7 @@ std::vector StopandWait::compose_trajectory_from_ { if (times[i] != 0 && !std::isnormal(times[i]) && i != 0) { // If the time - ROS_WARN_STREAM("Detected non-normal (nan, inf, etc.) time. Making it same as before: " << times[i-1]); + RCLCPP_WARN_STREAM(nh_->get_logger(),"Detected non-normal (nan, inf, etc.) time. Making it same as before: " << times[i-1]); // NOTE: overriding the timestamps in assumption that pure_pursuit_wrapper will detect it as stopping case times[i] = times[i - 1]; } @@ -381,18 +365,17 @@ std::vector StopandWait::compose_trajectory_from_ std::vector yaws = carma_wm::geometry::compute_tangent_orientations(raw_points); - for (size_t i = 0; i < points.size(); i++) { - ROS_DEBUG_STREAM("1d: " << downtracks[i] << " t: " << times[i] << " v: " << filtered_speeds[i]); + RCLCPP_DEBUG_STREAM(nh_->get_logger(),"1d: " << downtracks[i] << " t: " << times[i] << " v: " << filtered_speeds[i]); } auto traj = trajectory_from_points_times_orientations(raw_points, times, yaws, start_time); - while (traj.back().target_time - traj.front().target_time < ros::Duration(config_.minimal_trajectory_duration)) + while (rclcpp::Time(traj.back().target_time) - rclcpp::Time(traj.front().target_time) < rclcpp::Duration(config_.minimal_trajectory_duration * 1e9)) { - cav_msgs::TrajectoryPlanPoint new_point = traj.back(); - new_point.target_time = new_point.target_time + ros::Duration(config_.stop_timestep); + carma_planning_msgs::msg::TrajectoryPlanPoint new_point = traj.back(); + new_point.target_time = rclcpp::Time(new_point.target_time) + rclcpp::Duration(config_.stop_timestep * 1e9); traj.push_back(new_point); } diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp new file mode 100644 index 0000000000..96d9d6defc --- /dev/null +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp @@ -0,0 +1,72 @@ +/* + * 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 "stop_and_wait_node.hpp" + +namespace stop_and_wait_plugin +{ + namespace std_ph = std::placeholders; + + StopandWaitNode::StopandWaitNode(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::TacticalPlugin(options),version_id_("v1.0"),plugin_name_(get_plugin_name_and_ns()) + { + // Create initial config + config_ = StopandWaitConfig(); + + // Declare parameters + config_.minimal_trajectory_duration = declare_parameter("minimal_trajectory_duration", config_.minimal_trajectory_duration); + config_.stop_timestep = declare_parameter("stop_timestep", config_.stop_timestep); + config_.trajectory_step_size = declare_parameter("trajectory_step_size", config_.trajectory_step_size); + config_.accel_limit_multiplier = declare_parameter("accel_limit_multiplier", config_.accel_limit_multiplier); + config_.accel_limit = declare_parameter("/vehicle_acceleration_limit", config_.accel_limit); + config_.crawl_speed = declare_parameter("crawl_speed", config_.crawl_speed); + config_.centerline_sampling_spacing = declare_parameter("centerline_sampling_spacing", config_.centerline_sampling_spacing); + config_.default_stopping_buffer = declare_parameter("default_stopping_buffer", config_.default_stopping_buffer); + } + + carma_ros2_utils::CallbackReturn StopandWaitNode::on_configure_plugin() + { + + plugin_ = std::make_shared(shared_from_this(), wm_, config_,plugin_name_,version_id_); + + // Return success if everthing initialized successfully + return CallbackReturn::SUCCESS; + } + + void StopandWaitNode::plan_trajectory_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) + { + plugin_->plan_trajectory_cb(req, resp); + } + + bool StopandWaitNode::get_availability() + { + return true; + } + + std::string StopandWaitNode::get_version_id() + { + return version_id_; + } + +} // stop_and_wait_plugin + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(stop_and_wait_plugin::StopandWaitNode) diff --git a/stop_and_wait_plugin/test/test_stopandwait_plugin.cpp b/stop_and_wait_plugin/test/test_stopandwait_plugin.cpp index 930b3bf2ed..608e94f832 100644 --- a/stop_and_wait_plugin/test/test_stopandwait_plugin.cpp +++ b/stop_and_wait_plugin/test/test_stopandwait_plugin.cpp @@ -14,13 +14,12 @@ * the License. */ -#include "stop_and_wait_plugin.h" -#include "stop_and_wait_config.h" -#include -#include +#include "stop_and_wait_plugin.hpp" +#include "stop_and_wait_config.hpp" +#include #include #include -#include +#include #include #include #include @@ -32,21 +31,20 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include +#include #include -#include #include +#include namespace stop_and_wait_plugin { TEST(StopandWait, TestStopandWaitPlanning) { - ros::Time::setNow(ros::Time(0.0)); StopandWaitConfig config; config.minimal_trajectory_duration = 6.0; // Trajectory length in seconds @@ -56,7 +54,12 @@ TEST(StopandWait, TestStopandWaitPlanning) config.accel_limit = 2.0; // Longitudinal acceleration limit of the vehicle std::shared_ptr wm = std::make_shared(); - StopandWait plugin(wm, config, [&](auto msg) {}); + + std::shared_ptr nh; + const std::string& plugin_name= "stop_and_wait_plugin"; + const std::string& version_id="v1.0"; + + StopandWait plugin(nh,wm, config, plugin_name,version_id); auto map = carma_wm::test::buildGuidanceTestMap(3.7, 20); @@ -80,62 +83,63 @@ TEST(StopandWait, TestStopandWaitPlanning) carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - cav_srvs::PlanTrajectoryRequest req; - req.vehicle_state.x_pos_global = 1.5; - req.vehicle_state.y_pos_global = 5; - req.vehicle_state.orientation = 3.14/2; - req.vehicle_state.longitudinal_vel = 8.9408; // 20 mph - req.header.stamp = ros::Time(0.0); + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req; + req->vehicle_state.x_pos_global = 1.5; + req->vehicle_state.y_pos_global = 5; + req->vehicle_state.orientation = 3.14/2; + req->vehicle_state.longitudinal_vel = 8.9408; // 20 mph + req->header.stamp = rclcpp::Time(0.0* 1e9); - cav_msgs::Maneuver maneuver; - maneuver.type = cav_msgs::Maneuver::STOP_AND_WAIT; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT; maneuver.stop_and_wait_maneuver.start_dist = 5.0; - maneuver.stop_and_wait_maneuver.start_time = ros::Time(0.0); + maneuver.stop_and_wait_maneuver.start_time = rclcpp::Time(0.0* 1e9); maneuver.stop_and_wait_maneuver.start_speed = 8.9408; maneuver.stop_and_wait_maneuver.end_dist = 55; - maneuver.stop_and_wait_maneuver.end_time = ros::Time(11.175999999999998); + maneuver.stop_and_wait_maneuver.end_time = rclcpp::Time(11.175999999999998* 1e9); - req.maneuver_plan.maneuvers.push_back(maneuver); - req.maneuver_index_to_plan = 0; + req->maneuver_plan.maneuvers.push_back(maneuver); + req->maneuver_index_to_plan = 0; - cav_srvs::PlanTrajectoryResponse resp; + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp; plugin.plan_trajectory_cb(req, resp); double dist = 0; - double vel = resp.trajectory_plan.initial_longitudinal_velocity; + double vel = resp->trajectory_plan.initial_longitudinal_velocity; bool first= true; lanelet::BasicPoint2d prev_point; - ros::Time prev_time = ros::Time(0.0); - for (auto point : resp.trajectory_plan.trajectory_points) { + rclcpp::Time prev_time = rclcpp::Time(0.0* 1e9); + for (auto point : resp->trajectory_plan.trajectory_points) { lanelet::BasicPoint2d p(point.x, point.y); - ROS_INFO_STREAM("Y: " << point.y); + RCLCPP_INFO_STREAM(nh->get_logger(),"Y: " << point.y); double delta = 0; if (first) { first = false; } else { delta = lanelet::geometry::distance2d(p, prev_point); dist += delta; - ROS_INFO_STREAM("delta: " << delta << " timediff: " << (point.target_time - prev_time).toSec() << "pre_vel: " << vel); - vel = (2.0 * delta / (point.target_time - prev_time).toSec()) - vel; + RCLCPP_INFO_STREAM(nh->get_logger(),"delta: " << delta << " timediff: " << (rclcpp::Time(point.target_time) - prev_time).seconds() << "pre_vel: " << vel); + vel = (2.0 * delta / (rclcpp::Time(point.target_time) - prev_time).seconds()) - vel; } - ROS_ERROR_STREAM("point time: " << point.target_time.toSec() << " dist: " << dist << " vel: " << vel); + RCLCPP_ERROR_STREAM(nh->get_logger(),"point time: " << rclcpp::Time(point.target_time).seconds() << " dist: " << dist << " vel: " << vel); prev_point = p; prev_time = point.target_time; } } } // namespace stop_and_wait_plugin -// Run all the tests -int main(int argc, char** argv) +int main(int argc, char ** argv) { - testing::InitGoogleTest(&argc, argv); - ros::Time::init(); - ROSCONSOLE_AUTOINIT; - if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { // Change to Debug to enable debug logs - ros::console::notifyLoggerLevelsChanged(); - } - auto res = RUN_ALL_TESTS(); - - return res; -} \ No newline at end of file + ::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 From a01353f477c63096c2dfa0add3c99252233b7b54 Mon Sep 17 00:00:00 2001 From: Aswath <32398753+aswath1@users.noreply.github.com> Date: Mon, 25 Jul 2022 22:58:28 +0530 Subject: [PATCH 039/165] route_following_plugin_ros2 (#1835) Description Port the route_following_plugin node from ROS1 to ROS2 Related Issue #1856 Co-authored-by: Misheel Bayartsengel --- carma/launch/guidance.launch | 9 +- carma/launch/plugins.launch.py | 27 + route_following_plugin/CMakeLists.txt | 103 ++-- ...ng_plugin.h => route_following_plugin.hpp} | 149 +++--- .../include/route_following_plugin_config.hpp | 58 +++ .../launch/route_following_plugin.launch | 20 - .../launch/route_following_plugin_launch.py | 62 +++ route_following_plugin/package.xml | 33 +- route_following_plugin/src/main.cpp | 22 +- .../src/route_following_plugin.cpp | 483 +++++++++--------- .../test/test_route_following_plugin.cpp | 217 ++++---- .../test/test_stop_at_end_of_route.cpp | 158 +++--- 12 files changed, 715 insertions(+), 626 deletions(-) rename route_following_plugin/include/{route_following_plugin.h => route_following_plugin.hpp} (71%) create mode 100644 route_following_plugin/include/route_following_plugin_config.hpp delete mode 100644 route_following_plugin/launch/route_following_plugin.launch create mode 100644 route_following_plugin/launch/route_following_plugin_launch.py diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index d6740db383..c39975ec6f 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -131,14 +131,7 @@ - - - - - - - - + diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 9ee9327390..3cc4a604bf 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -47,6 +47,9 @@ def generate_launch_description(): inlanecruising_plugin_file_path = os.path.join( get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml') + route_following_plugin_file_path = os.path.join( + get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') + stop_and_wait_plugin_param_file = os.path.join( get_package_share_directory('stop_and_wait_plugin'), 'config/stop_and_wait_plugin_params.yaml') @@ -99,6 +102,30 @@ def generate_launch_description(): stop_and_wait_plugin_param_file, vehicle_config_param_file ] + ), + ComposableNode( + package='route_following_plugin', + plugin='route_following_plugin::RouteFollowingPlugin', + name='route_following_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('route_following_plugin', env_log_levels) } + ], + 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" ] ), + ("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" ] ), + ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ("maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/final_maneuver_plan" ] ), + ("upcoming_lane_change_status", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/upcoming_lane_change_status" ] ), + ], + parameters=[ + route_following_plugin_file_path, + vehicle_config_param_file + ] ) ] ) diff --git a/route_following_plugin/CMakeLists.txt b/route_following_plugin/CMakeLists.txt index bd88472f87..8fa36a96a0 100644 --- a/route_following_plugin/CMakeLists.txt +++ b/route_following_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2018-2021 LEIDOS. +# 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 @@ -14,91 +14,56 @@ # the License. # -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(route_following_plugin) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - cav_msgs - cav_srvs - carma_utils - carma_wm - tf - tf2 - tf2_geometry_msgs -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - -################################### -## catkin specific configuration ## -################################### +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -catkin_package( - CATKIN_DEPENDS roscpp std_msgs cav_srvs carma_utils cav_msgs carma_wm tf tf2 tf2_geometry_msgs -) - -########### -## Build ## -########### +# Name build targets +set(node_exec route_following_plugin_exec) +set(node_lib route_following_plugin_lib) include_directories( - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} include ) +# Build +ament_auto_add_library(${node_lib} SHARED + src/route_following_plugin.cpp +) -add_executable( ${PROJECT_NAME} - src/route_following_plugin.cpp - src/main.cpp) +ament_auto_add_executable(${node_exec} + src/main.cpp +) -add_library(route_following_plugin_lib src/route_following_plugin.cpp) -add_dependencies(route_following_plugin_lib ${catkin_EXPORTED_TARGETS}) +# Register component +rclcpp_components_register_nodes(${node_lib} "route_following_plugin::RouteFollowingPlugin") -target_link_libraries(route_following_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} +) -target_link_libraries(${PROJECT_NAME} route_following_plugin_lib) +# Testing +if(BUILD_TESTING) -############# -## Install ## -############# + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) + ament_add_gtest(test_route_following_plugin test/test_route_following_plugin.cpp test/test_stop_at_end_of_route.cpp) + ament_target_dependencies(test_route_following_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + target_link_libraries(test_route_following_plugin ${node_lib}) -## Install C++ -install(TARGETS ${PROJECT_NAME} route_following_plugin_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +endif() -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch ) - -############# -## Testing ## -############# -catkin_add_gmock(${PROJECT_NAME}-test -test/test_route_following_plugin.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) - -catkin_add_gmock(${PROJECT_NAME}-stopping-test -test/test_stop_at_end_of_route.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) - -target_link_libraries(${PROJECT_NAME}-test route_following_plugin_lib ${catkin_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}-stopping-test route_following_plugin_lib ${catkin_LIBRARIES}) diff --git a/route_following_plugin/include/route_following_plugin.h b/route_following_plugin/include/route_following_plugin.hpp similarity index 71% rename from route_following_plugin/include/route_following_plugin.h rename to route_following_plugin/include/route_following_plugin.hpp index 4d6ab6937f..7f110a2e6d 100644 --- a/route_following_plugin/include/route_following_plugin.h +++ b/route_following_plugin/include/route_following_plugin.hpp @@ -17,19 +17,22 @@ */ #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include #include #include +#include "carma_guidance_plugins/strategic_plugin.hpp" +#include "route_following_plugin_config.hpp" /** * \brief Macro definition to enable easier access to fields shared across the maneuver types @@ -38,46 +41,35 @@ * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field */ #define GET_MANEUVER_PROPERTY(mvr, property)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\ throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))))) #define SET_MANEUVER_PROPERTY(mvr, property, value)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property = (value) :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property = (value) :\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property = (value) :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property = (value) :\ throw std::invalid_argument("SET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))))) - namespace route_following_plugin { - - class RouteFollowingPlugin + class RouteFollowingPlugin : public carma_guidance_plugins::StrategicPlugin { public: /** * \brief Default constructor for RouteFollowingPlugin class */ - RouteFollowingPlugin() = default; - - - /** - * \brief General entry point to begin the operation of this class - */ - void run(); - - /** - * \brief Initialize ROS publishers, subscribers, service servers and service clients - */ - void initialize(); + explicit RouteFollowingPlugin(const rclcpp::NodeOptions &); + + carma_ros2_utils::CallbackReturn on_configure_plugin(); // wm listener pointer and pointer to the actual wm object std::shared_ptr wml_; @@ -90,7 +82,10 @@ namespace route_following_plugin * \return UpcomingLaneChangeStatus Note: this method will only work correctly if the * two provided lanelets are forming a lane change */ - cav_msgs::UpcomingLaneChangeStatus ComposeLaneChangeStatus(lanelet::ConstLanelet starting_lanelet,lanelet::ConstLanelet ending_lanelet); + carma_planning_msgs::msg::UpcomingLaneChangeStatus ComposeLaneChangeStatus(lanelet::ConstLanelet starting_lanelet,lanelet::ConstLanelet ending_lanelet); + + bool get_availability(); + std::string get_version_id(); private: @@ -103,7 +98,7 @@ namespace route_following_plugin * \param lane_ids List of lanelet IDs that the current maneuver traverses. Message expects these to be contiguous and end to end * \return A lane keeping maneuver message which is ready to be published */ - cav_msgs::Maneuver composeLaneFollowingManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, const std::vector& lane_ids) const; + carma_planning_msgs::msg::Maneuver composeLaneFollowingManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, const std::vector& lane_ids) const; /** * \brief Compose a lane change maneuver message based on input params @@ -116,7 +111,7 @@ namespace route_following_plugin * \param target_speed Target speed of the current maneuver * \return A lane keeping maneuver message which is ready to be published */ - cav_msgs::Maneuver composeLaneChangeManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id,lanelet::Id ending_lane_id) const; + carma_planning_msgs::msg::Maneuver composeLaneChangeManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id,lanelet::Id ending_lane_id) const; /** * \brief Compose a stop and wait maneuver message based on input params. @@ -129,7 +124,7 @@ namespace route_following_plugin * \param stopping_accel Acceleration used for calculating the stopping distance * \return A lane keeping maneuver message which is ready to be published */ - cav_msgs::Maneuver composeStopAndWaitManeuverMessage(double start_dist, double end_dist, double start_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double stopping_accel) const; + carma_planning_msgs::msg::Maneuver composeStopAndWaitManeuverMessage(double start_dist, double end_dist, double start_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double stopping_accel) const; /** * \brief Given a LaneletRelations and ID of the next lanelet in the shortest path @@ -144,20 +139,20 @@ namespace route_following_plugin * \param maneuver A maneuver (non-specific to type) to be performed * \param start_dist the starting distance that the maneuver need to be updated to */ - void setManeuverStartDist(cav_msgs::Maneuver& maneuver, double start_dist) const; + void setManeuverStartDist(carma_planning_msgs::msg::Maneuver& maneuver, double start_dist) const; /** * \brief Given an array of maneuvers update the starting time for each * \param maneuvers An array of maneuvers (non-specific to type) * \param start_time The starting time for the first maneuver in the sequence, each consequent maneuver is pushed ahead by same amount */ - void updateTimeProgress(std::vector& maneuvers, ros::Time start_time) const; + void updateTimeProgress(std::vector& maneuvers, rclcpp::Time start_time) const; /** * \brief Given an maneuver update the starting speed * \param maneuver maneuver to update the starting speed for * \param start_time The starting speed for the maneuver passed as argument */ - void updateStartingSpeed(cav_msgs::Maneuver& maneuver, double start_speed) const; + void updateStartingSpeed(carma_planning_msgs::msg::Maneuver& maneuver, double start_speed) const; /** * \brief Service callback for arbitrator maneuver planning @@ -165,7 +160,11 @@ namespace route_following_plugin * \param resp Plan maneuver response with a list of maneuver plan * \return If service call successed */ - bool planManeuverCb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp); + + void plan_maneuvers_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp); /** * \brief Given a Lanelet, find it's associated Speed Limit @@ -178,7 +177,7 @@ namespace route_following_plugin * \brief Calculate maneuver plan for remaining route. This callback is triggered when a new route has been received and processed by the world model * \param route_shortest_path A list of lanelets along the shortest path of the route using which the maneuver plan is calculated. */ - std::vector routeCb(const lanelet::routing::LaneletPath& route_shortest_path); + std::vector routeCb(const lanelet::routing::LaneletPath& route_shortest_path); /** * \brief Adds a StopAndWait maneuver to the end of a maneuver set stopping at the provided downtrack value @@ -201,8 +200,8 @@ namespace route_following_plugin * that any previous maneuvers which were slower need not be accounted for as planning for a higher speed will always be capable of handling that case * and any which were faster would already have their speed reduced by the maneuver which this speed was derived from. */ - std::vector addStopAndWaitAtRouteEnd ( - const std::vector& input_maneuvers, + std::vector addStopAndWaitAtRouteEnd ( + const std::vector& input_maneuvers, double route_end_downtrack, double stopping_entry_speed, double stopping_logitudinal_accel, double lateral_accel_limit, double min_maneuver_length ) const; @@ -217,7 +216,7 @@ namespace route_following_plugin * * \return true if the provided maneuver plus the computed dynamic buffer starts after the provided downtrack value */ - bool maneuverWithBufferStartsAfterDowntrack(const cav_msgs::Maneuver& maneuver, double downtrack, double lateral_accel, double min_maneuver_length) const; + bool maneuverWithBufferStartsAfterDowntrack(const carma_planning_msgs::msg::Maneuver& maneuver, double downtrack, double lateral_accel, double min_maneuver_length) const; /** * \brief This method returns a new UUID as a string for assignment to a Maneuver message @@ -234,42 +233,26 @@ namespace route_following_plugin */ void returnToShortestPath(const lanelet::ConstLanelet ¤t_lanelet); - // CARMA ROS node handles - std::shared_ptr nh_, pnh_; - - // ROS publishers and subscribers - ros::Publisher plugin_discovery_pub_; - ros::Publisher upcoming_lane_change_status_pub_; - ros::Subscriber twist_sub_; - ros::Subscriber current_maneuver_plan_sub_; - ros::Timer discovery_pub_timer_; + //Subscribers + carma_ros2_utils::SubPtr twist_sub_; + carma_ros2_utils::SubPtr current_maneuver_plan_sub_; + + // Publishers + carma_ros2_utils::PubPtr upcoming_lane_change_status_pub_; - // ROS service servers - ros::ServiceServer plan_maneuver_srv_; + // Service Servers + carma_ros2_utils::ServicePtr plan_maneuver_srv_; // unordered set of all the lanelet ids in shortest path std::unordered_set shortest_path_set_; - - // Minimal duration of maneuver, loaded from config file - double min_plan_duration_ = 16.0; - - double route_end_point_buffer_ = 10.0; - - double stopping_accel_limit_multiplier_ = 0.5; - - double accel_limit_ = 2.0; - - double lateral_accel_limit_ = 2.0; - - double min_maneuver_length_ = 10.0; // Minimum length to allow for a maneuver when updating it for stop and wait - + static constexpr double MAX_LANE_WIDTH = 3.70; // Maximum lane width of a US highway - // Plugin discovery message - cav_msgs::Plugin plugin_discovery_msg_; + // Node configuration + Config config_; //Upcoming Lane Change downtrack and its lanechange status message map - std::queue> upcoming_lane_change_status_msg_map_; + std::queue> upcoming_lane_change_status_msg_map_; // Current vehicle forward speed double current_speed_ = 0.0; @@ -278,14 +261,14 @@ namespace route_following_plugin double epsilon_ = 0.0001; // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; + geometry_msgs::msg::PoseStamped pose_msg_; lanelet::BasicPoint2d current_loc_; // Currently executing maneuver plan from Arbitrator - cav_msgs::ManeuverPlanConstPtr current_maneuver_plan_; + carma_planning_msgs::msg::ManeuverPlan::UniquePtr current_maneuver_plan_; //Queue of maneuver plans - std::vector latest_maneuver_plan_; + std::vector latest_maneuver_plan_; //Tactical plugin being used for planning lane change std::string lane_change_plugin_ = "cooperative_lanechange"; @@ -293,7 +276,7 @@ namespace route_following_plugin std::string planning_strategic_plugin_ = "route_following_plugin"; std::string lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"; - + /** * \brief Callback for the front bumper pose transform */ @@ -303,7 +286,7 @@ namespace route_following_plugin * \brief Callback for the twist subscriber, which will store latest twist locally * \param msg Latest twist message */ - void twist_cb(const geometry_msgs::TwistStampedConstPtr& msg); + void twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg); /** * \brief Callback for the ManeuverPlan subscriber, will store the current maneuver plan received locally. @@ -311,7 +294,7 @@ namespace route_following_plugin * vs. control drift taking the vehicle's reference point outside of the intended lane. * \param msg Latest ManeuverPlan message */ - void current_maneuver_plan_cb(const cav_msgs::ManeuverPlanConstPtr& msg); + void current_maneuver_plan_cb(carma_planning_msgs::msg::ManeuverPlan::UniquePtr msg); /** * \brief returns duration as ros::Duration required to complete maneuver given its start dist, end dist, start speed and end speed @@ -319,14 +302,14 @@ namespace route_following_plugin * \param epsilon The acceptable min start_speed + target_speed in the maneuver message, under which the maneuver is treated as faulty. * Throws exception if sum of start and target speed of maneuver is below limit defined by parameter epsilon */ - ros::Duration getManeuverDuration(cav_msgs::Maneuver &maneuver, double epsilon) const; + rclcpp::Duration getManeuverDuration(carma_planning_msgs::msg::Maneuver &maneuver, double epsilon) const; /** * \brief Initialize transform lookup from front bumper to map */ void initializeBumperTransformLookup(); - geometry_msgs::TransformStamped tf_; + geometry_msgs::msg::TransformStamped tf_; // front bumper transform tf2::Stamped frontbumper_transform_; diff --git a/route_following_plugin/include/route_following_plugin_config.hpp b/route_following_plugin/include/route_following_plugin_config.hpp new file mode 100644 index 0000000000..16e4a8e880 --- /dev/null +++ b/route_following_plugin/include/route_following_plugin_config.hpp @@ -0,0 +1,58 @@ +#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. + */ + +#include + +namespace route_following_plugin +{ + /** + * \brief Struct containing config values values for route_following_plugin + */ + + struct Config + { + // Minimal duration of maneuver, loaded from config file + double min_plan_duration_ = 16.0; + //Tactical plugin being used for planning lane change + std::string lane_change_plugin_ = "cooperative_lanechange"; + std::string stop_and_wait_plugin_ = "stop_and_wait_plugin"; + + std::string lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"; + double route_end_point_buffer_ = 10.0; + double accel_limit_ = 2.0; + double lateral_accel_limit_ = 2.0; + double stopping_accel_limit_multiplier_ = 0.5; + double min_maneuver_length_ = 10.0; // Minimum length to allow for a maneuver when updating it for stop and wait + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "route_following_plugin::Config { " << std::endl + << "min_plan_duration_: " << c.min_plan_duration_ << std::endl + << "lane_change_plugin_: " << c.lane_change_plugin_ << std::endl + << "stop_and_wait_plugin_: " << c.stop_and_wait_plugin_ << std::endl + << "lanefollow_planning_tactical_plugin_: " << c.lanefollow_planning_tactical_plugin_ << std::endl + << "route_end_point_buffer_: " << c.route_end_point_buffer_ << std::endl + << "accel_limit_: " << c.accel_limit_ << std::endl + << "lateral_accel_limit_: " << c.lateral_accel_limit_ << std::endl + << "stopping_accel_limit_multiplier_: " << c.stopping_accel_limit_multiplier_ << std::endl + << "min_maneuver_length_: " << c.min_maneuver_length_ << std::endl + << "}" << std::endl; + return output; + } + }; +} // route_following_plugin \ No newline at end of file diff --git a/route_following_plugin/launch/route_following_plugin.launch b/route_following_plugin/launch/route_following_plugin.launch deleted file mode 100644 index 09bfe8dfd2..0000000000 --- a/route_following_plugin/launch/route_following_plugin.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - diff --git a/route_following_plugin/launch/route_following_plugin_launch.py b/route_following_plugin/launch/route_following_plugin_launch.py new file mode 100644 index 0000000000..9436bddd06 --- /dev/null +++ b/route_following_plugin/launch/route_following_plugin_launch.py @@ -0,0 +1,62 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA route_following_plugin_container. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='route_following_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='route_following_plugin', + plugin='route_following_plugin::RouteFollowingPlugin', + name='route_following_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/route_following_plugin/package.xml b/route_following_plugin/package.xml index 7f5bccc12f..7874de09a4 100644 --- a/route_following_plugin/package.xml +++ b/route_following_plugin/package.xml @@ -1,7 +1,7 @@ Staging Area Entrance + # 1. Staging Area Entrance ----> Staging Area Pickup + # 2. Staging Area Pickup ------> Staging Area Exit + # 3. Staging Area Exit --------> Port Entrance + # 4. Port Entrance ------------> Port Dropoff + # 5. Port Dropoff -------------> Port Pickup + # 6. Port Pickup --------------> Port Checkpoint + # 7. Port Checkpoint ----------> Port Holding Area + # 8. Port Holding Area --------> Port Exit + # 9. Port Exit ----------------> Staging Area Entrance + num_engagement_sections = 10 # 10 expected engagement sections for the Port Drayage use case + start_times_list, end_times_list, found_engagement_times = get_test_case_engagement_times(bag, num_engagement_sections) + segment_0 = [start_times_list[0], end_times_list[0], 0] # Test Start Location ------> Staging Area Entrance + segment_1 = [start_times_list[1], end_times_list[1], 1] # Staging Area Entrance ----> Staging Area Pickup + segment_2 = [start_times_list[2], end_times_list[2], 2] # Staging Area Pickup ------> Staging Area Exit + segment_3 = [start_times_list[3], end_times_list[3], 3] # Staging Area Exit --------> Port Entrance + segment_4 = [start_times_list[4], end_times_list[4], 4] # Port Entrance ------------> Port Dropoff + segment_5 = [start_times_list[5], end_times_list[5], 5] # Port Dropoff -------------> Port Pickup + segment_6 = [start_times_list[6], end_times_list[6], 6] # Port Pickup --------------> Port Checkpoint + segment_7 = [start_times_list[7], end_times_list[7], 7] # Port Checkpoint ----------> Port Holding Area + segment_8 = [start_times_list[8], end_times_list[8], 8] # Port Holding Area --------> Port Exit + segment_9 = [start_times_list[9], end_times_list[9], 9] # Port Exit ----------------> Staging Area Entrance + + print_lanelet_entrance_times(bag, segment_0[0]) + # Finished: + # 1 ( 0, , , , , , , , , ) + # 4 ( 0, , , 3, , , , , ,I9) + # 19 ( 1, 2, , 4, 5, 6, 7, 8, ) + # 23 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, 9) + # 24 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, ) + # 25 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, ) + # 18 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, ) # CMV sends arrival message + # 9 ( 0, , 2, 3, , , , , 8, ) # V2XHub sends next destination after arrival + # 20 (I0, 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV can receive MobilityOperation messages from V2XHub + # 10 (I0, 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV receives the next destination from V2XHub + # 21 ( , 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV receives next destination < 1.5 sec after sending arrival + # 22 (I0, 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV generates route to next destination in < 3.0 sec + # 8 ( , 1, , , 4, 5, 6, 7, 8, ) + # 2 ( 0, , , 3, , , , , , ) + # 11 ( , 1, , , 4, 5, , , , ) + # 5 ( , , , , , , 6, , , ) + # 7 ( , , , , , , , 7, , ) + # 3 ( , , , 3, , , , , , 9) + # 6 ( , , , , , ,V6, , , ) # Requires V2XHub Logs (Test Operator can select either Holding Area or Port Exit) + # 12 ( ,V1, , , ,V5, , , , ) # Requires V2XHub Logs (Test Operator Communicates to V2XHub that container is loaded) + # 13 ( , 1, , , , 5, , , , ) + # 14 ( , , , , 4, , , , , ) + # 15 ( ,V1, , , ,V5, , , , ) # Requires V2XHub Logs (V2XHub sends loading command to Test Operator) + # 16 ( , , , ,V4, , , , , ) # Requires V2XHub Logs (V2XHub sends unloading command to Test Operator) + # 17 ( , , , ,V4, , , , , ) # Requires V2XHub Logs (Test Operator Communicates to V2XHub that container is unloaded) + + # Metric FPD-1 (Vehicle reaches +/- 2 mph of speed limit) + speed_limit_outside_port_and_staging_area_ms = 8.94 # 8.94 m/s is 20 mph + pd_1_result_0 = check_vehicle_reaches_steady_state(bag, segment_0, speed_limit_outside_port_and_staging_area_ms) + + # Metric FPD-4 (Speed limit outside Staging Area & Port is 20-25 mph) + min_speed_limit_FPD4 = 8.94 # 8.94 m/s is 20 mph + max_speed_limit_FPD4 = 11.18 # 11.18 m/s is 25 mph + segment_0_lanelet_ids = [65787, 24943, 76157] # Ordered lanelet IDs encountered in this segment that are outside of Staging Area and Port + segment_3_lanelet_ids = [75649, 25151, 66085, 16359, 18269, 82313] # Ordered lanelet IDs encountered in this segment that are outside of Staging Area and Port + pd_4_result_0 = check_speed_limit_outside_port_and_staging_area(bag, segment_0, segment_0_lanelet_ids, min_speed_limit_FPD4, max_speed_limit_FPD4) + pd_4_result_3 = check_speed_limit_outside_port_and_staging_area(bag, segment_3, segment_3_lanelet_ids, min_speed_limit_FPD4, max_speed_limit_FPD4) + + # FPD-19 (Speed limit inside Staging Area or Port is 10 mph) + speed_limit_inside_port_and_staging_area_ms = 4.47 # 4.47 m/s is 10 mph + pd_19_result_1 = check_speed_limit_inside_port_and_staging_area(bag, segment_1, speed_limit_inside_port_and_staging_area_ms) + pd_19_result_2 = check_speed_limit_inside_port_and_staging_area(bag, segment_2, speed_limit_inside_port_and_staging_area_ms) + pd_19_result_4 = check_speed_limit_inside_port_and_staging_area(bag, segment_4, speed_limit_inside_port_and_staging_area_ms) + pd_19_result_5 = check_speed_limit_inside_port_and_staging_area(bag, segment_5, speed_limit_inside_port_and_staging_area_ms) + pd_19_result_6 = check_speed_limit_inside_port_and_staging_area(bag, segment_6, speed_limit_inside_port_and_staging_area_ms) + pd_19_result_7 = check_speed_limit_inside_port_and_staging_area(bag, segment_7, speed_limit_inside_port_and_staging_area_ms) + pd_19_result_8 = check_speed_limit_inside_port_and_staging_area(bag, segment_8, speed_limit_inside_port_and_staging_area_ms) + + # FPD-23 (Performance requirements for acceleration at start of route) + pd_23_result_0 = check_acceleration_after_engagement(bag, segment_0, speed_limit_outside_port_and_staging_area_ms) + pd_23_result_1 = check_acceleration_after_engagement(bag, segment_1, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_2 = check_acceleration_after_engagement(bag, segment_2, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_3 = check_acceleration_after_engagement(bag, segment_3, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_4 = check_acceleration_after_engagement(bag, segment_4, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_5 = check_acceleration_after_engagement(bag, segment_5, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_6 = check_acceleration_after_engagement(bag, segment_6, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_7 = check_acceleration_after_engagement(bag, segment_7, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_8 = check_acceleration_after_engagement(bag, segment_8, speed_limit_inside_port_and_staging_area_ms) + pd_23_result_9 = check_acceleration_after_engagement(bag, segment_9, speed_limit_inside_port_and_staging_area_ms) + + # FPD-24 (Performance requirements for deceleration at end of route) + pd_24_result_0 = check_deceleration_at_end_of_route(bag, segment_0) + pd_24_result_1 = check_deceleration_at_end_of_route(bag, segment_1) + pd_24_result_2 = check_deceleration_at_end_of_route(bag, segment_2) + pd_24_result_3 = check_deceleration_at_end_of_route(bag, segment_3) + pd_24_result_4 = check_deceleration_at_end_of_route(bag, segment_4) + pd_24_result_5 = check_deceleration_at_end_of_route(bag, segment_5) + pd_24_result_6 = check_deceleration_at_end_of_route(bag, segment_6) + pd_24_result_7 = check_deceleration_at_end_of_route(bag, segment_7) + pd_24_result_8 = check_deceleration_at_end_of_route(bag, segment_8) + + # FPD-25 (Route Completed notification occurs within 3 seconds of reaching destination) + pd_25_result_0 = check_route_completed_notification(bag, segment_0) + pd_25_result_1 = check_route_completed_notification(bag, segment_1) + pd_25_result_2 = check_route_completed_notification(bag, segment_2) + pd_25_result_3 = check_route_completed_notification(bag, segment_3) + pd_25_result_4 = check_route_completed_notification(bag, segment_4) + pd_25_result_5 = check_route_completed_notification(bag, segment_5) + pd_25_result_6 = check_route_completed_notification(bag, segment_6) + pd_25_result_7 = check_route_completed_notification(bag, segment_7) + pd_25_result_8 = check_route_completed_notification(bag, segment_8) + + # FPD-18 (CMV broadcasts arrival message to infrastructure) + # Do one function that takes the 'OPERATION' as a passed argument + pd_18_result_0, pd_9_result_0, pd_20_result_1, pd_10_result_1, pd_21_result_1, pd_22_result_1 = check_cmv_arrival_message(bag, segment_0, "\"operation\": \"ENTER_STAGING_AREA\"", "\"operation\": \"PICKUP\"") + pd_18_result_1, pd_9_result_1, pd_20_result_2, pd_10_result_2, pd_21_result_2, pd_22_result_2 = check_cmv_arrival_message(bag, segment_1, "\"operation\": \"PICKUP\"", "\"operation\": \"EXIT_STAGING_AREA\"") + pd_18_result_2, pd_9_result_2, pd_20_result_3, pd_10_result_3, pd_21_result_3, pd_22_result_3 = check_cmv_arrival_message(bag, segment_2, "\"operation\": \"EXIT_STAGING_AREA\"", "\"operation\": \"ENTER_PORT\"") + pd_18_result_3, pd_9_result_3, pd_20_result_4, pd_10_result_4, pd_21_result_4, pd_22_result_4 = check_cmv_arrival_message(bag, segment_3, "\"operation\": \"ENTER_PORT\"", "\"operation\": \"DROPOFF\"") + pd_18_result_4, pd_9_result_4, pd_20_result_5, pd_10_result_5, pd_21_result_5, pd_22_result_5 = check_cmv_arrival_message(bag, segment_4, "\"operation\": \"DROPOFF\"", "\"operation\": \"PICKUP\"") + pd_18_result_5, pd_9_result_5, pd_20_result_6, pd_10_result_6, pd_21_result_6, pd_22_result_6 = check_cmv_arrival_message(bag, segment_5, "\"operation\": \"PICKUP\"", "\"operation\": \"PORT_CHECKPOINT\"") + pd_18_result_6, pd_9_result_6, pd_20_result_7, pd_10_result_7, pd_21_result_7, pd_22_result_7 = check_cmv_arrival_message(bag, segment_6, "\"operation\": \"PORT_CHECKPOINT\"", "\"operation\": \"HOLDING_AREA\"") + pd_18_result_7, pd_9_result_7, pd_20_result_8, pd_10_result_8, pd_21_result_8, pd_22_result_8 = check_cmv_arrival_message(bag, segment_7, "\"operation\": \"HOLDING_AREA\"", "\"operation\": \"EXIT_PORT\"") + pd_18_result_8, pd_9_result_8, pd_20_result_9, pd_10_result_9, pd_21_result_9, pd_22_result_9 = check_cmv_arrival_message(bag, segment_8, "\"operation\": \"EXIT_PORT\"", "\"operation\": \"ENTER_STAGING_AREA\"") + + # Check FPD-13 Segments 1 and 5 (Infrastructure communicates to CMV that container is loaded) + if (pd_10_result_2): + pd_13_result_1 = True + print("PD-13-1 Succeeded; CMV was notified that container was loaded.") + else: + pd_13_result_1 = False + print("PD-13-1 Failed; CMV was not notified that container was loaded.") + + if (pd_10_result_6): + pd_13_result_5 = True + print("PD-13-5 Succeeded; CMV was notified that container was loaded.") + else: + pd_13_result_5 = False + print("PD-13-5 Failed; CMV was not notified that container was loaded.") + + # Check FPD-14 Segment 4 (Infrastructure communicates to CMV that container is unloaded) + if (pd_10_result_5): + pd_14_result_4 = True + print("PD-14-4 Succeeded; CMV was notified that container was loaded.") + else: + pd_14_result_4 = False + print("PD-14-4 Failed; CMV was not notified that container was loaded.") + + # PD-8 is always true (System engages on route after user chooses to engage from the Web UI) + # PD-9 is always true (System can exit Staging Area or Port after receiving command from V2XHub) + # Note: These metrics were evaluated in the vehicle, not by the data analysis script. + pd_8_result_1 = True + pd_3_result_3 = True + pd_8_result_4 = True + pd_8_result_5 = True + pd_8_result_6 = True + pd_8_result_7 = True + pd_8_result_8 = True + pd_3_result_9 = True + + # FPD-2, FPD-11, and FPD-5 (stopping location metrics) + # Note: These map stopping locations were obtained by using the Lat/Long stopping locations from the + # use case and transforming them to the map frame via unit tests contained in the carma-platform Route package. + stop_location_0 = [-401.128, 515.763] # [map_x, map_y] + stop_location_1 = [-414.198, 709.169] # [map_x, map_y] + stop_location_2 = [-408.123, 514.201] # [map_x, map_y] # No metric exists to evaluate this stop + stop_location_3 = [-75.2801, -647.096] # [map_x, map_y] + stop_location_4 = [-52.0422, -731.793] # [map_x, map_y] + stop_location_5 = [-28.6465, -758.279] # [map_x, map_y] + stop_location_6 = [2.51176, -723.625] # [map_x, map_y] + stop_location_7 = [-3.0006, -647.181] # [map_x, map_y] + stop_location_8 = [-112.047, -506.881] # [map_x, map_y] # No metric exists to evaluate this stop + + freightliner_vehicle_length = 6.0 + pd_2_result_0 = check_stop_location(bag, segment_0, stop_location_0, max_distance = 3, metric_number = 2, vehicle_length = freightliner_vehicle_length) + pd_11_result_1 = check_stop_location(bag, segment_1, stop_location_1, max_distance = 10, metric_number = 11, vehicle_length = freightliner_vehicle_length) + pd_2_result_3 = check_stop_location(bag, segment_3, stop_location_3, max_distance = 3, metric_number = 2, vehicle_length = freightliner_vehicle_length) + pd_11_result_4 = check_stop_location(bag, segment_4, stop_location_4, max_distance = 10, metric_number = 11, vehicle_length = freightliner_vehicle_length) + pd_11_result_5 = check_stop_location(bag, segment_5, stop_location_5, max_distance = 10, metric_number = 11, vehicle_length = freightliner_vehicle_length) + pd_5_result_6 = check_stop_location(bag, segment_6, stop_location_6, max_distance = 3, metric_number = 5, vehicle_length = freightliner_vehicle_length) + pd_7_result_7 = check_stop_location(bag, segment_7, stop_location_7, max_distance = 3, metric_number = 7, vehicle_length = freightliner_vehicle_length) + + # Invalid Metrics (Either not-applicable or the metrics are not for carma-platform): + pd_4_result_9 = None + pd_20_result_0 = None + pd_10_result_0 = None + pd_22_result_0 = None + pd_6_result_6 = None + pd_12_result_1 = None + pd_12_result_5 = None + pd_15_result_1 = None + pd_15_result_5 = None + pd_16_result_4 = None + pd_17_result_4 = None + + # Get vehicle type that this bag file is from + vehicle_name = "Unknown" + if bag_file in silver_truck_bag_files: + vehicle_name = "Silver Truck" + else: + vehicle_name = "N/A" + + # Get test type that this bag file is for + vehicle_role = "Port Drayage" + + # Write simple pass/fail results to .csv file for appropriate row: + csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, + pd_1_result_0, + pd_4_result_0, pd_4_result_3, pd_4_result_9, + pd_19_result_1, pd_19_result_2, pd_19_result_4, pd_19_result_5, pd_19_result_6, pd_19_result_7, pd_19_result_8, + pd_23_result_0, pd_23_result_1, pd_23_result_2, pd_23_result_3, pd_23_result_4, pd_23_result_5, pd_23_result_6, pd_23_result_7, pd_23_result_8, pd_23_result_9, + pd_24_result_0, pd_24_result_1, pd_24_result_2, pd_24_result_3, pd_24_result_4, pd_24_result_5, pd_24_result_6, pd_24_result_7, pd_24_result_8, + pd_25_result_0, pd_25_result_1, pd_25_result_2, pd_25_result_3, pd_25_result_4, pd_25_result_5, pd_25_result_6, pd_25_result_7, pd_25_result_8, + pd_18_result_0, pd_18_result_1, pd_18_result_2, pd_18_result_3, pd_18_result_4, pd_18_result_5, pd_18_result_6, pd_18_result_7, pd_18_result_8, + pd_9_result_0, pd_9_result_2, pd_9_result_3, pd_9_result_8, + pd_20_result_0, pd_20_result_1, pd_20_result_2, pd_20_result_3, pd_20_result_4, pd_20_result_5, pd_20_result_6, pd_20_result_7, pd_20_result_8, pd_20_result_9, + pd_10_result_0, pd_10_result_1, pd_10_result_2, pd_10_result_3, pd_10_result_4, pd_10_result_5, pd_10_result_6, pd_10_result_7, pd_10_result_8, pd_10_result_9, + pd_21_result_1, pd_21_result_2, pd_21_result_3, pd_21_result_4, pd_21_result_5, pd_21_result_6, pd_21_result_7, pd_21_result_8, pd_21_result_9, + pd_22_result_0, pd_22_result_1, pd_22_result_2, pd_22_result_3, pd_22_result_4, pd_22_result_5, pd_22_result_6, pd_22_result_7, pd_22_result_8, pd_22_result_9, + pd_8_result_1, pd_8_result_4, pd_8_result_5, pd_8_result_6, pd_8_result_7, pd_8_result_8, + pd_2_result_0, pd_2_result_3, + pd_11_result_1, pd_11_result_4, pd_11_result_5, + pd_5_result_6, + pd_7_result_7, + pd_3_result_3, pd_3_result_9, + pd_6_result_6, + pd_12_result_1, pd_12_result_5, + pd_13_result_1, pd_13_result_5, + pd_14_result_4, + pd_15_result_1, pd_15_result_5, + pd_16_result_4, + pd_17_result_4]) + + sys.stdout = orig_stdout + text_log_file_writer.close() + return + +if __name__ == "__main__": + main() diff --git a/engineering_tools/data_processing/analyze_tsmo_uc2_rosbags.py b/engineering_tools/data_processing/analyze_tsmo_uc2_rosbags.py new file mode 100644 index 0000000000..c9006dc95c --- /dev/null +++ b/engineering_tools/data_processing/analyze_tsmo_uc2_rosbags.py @@ -0,0 +1,443 @@ +#!/usr/bin/python3 + +# 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. + +from cmath import phase +from inspect import TPFLAGS_IS_ABSTRACT +import sys +import csv +import matplotlib.pyplot as plt +import rospy +import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" +import datetime +import math + +########################################################################################################### +# TSMO UC1-M1.1 : Vehicles should receive SPaT messages from RSU +# once the vehicles enter the communication area until the vehicles depart the intersection box. +# Each vehicle processes this information and use it as inputs of the trajectory planning-related features +# (i.e., TS, the current CARMA Platform path following feature). +# The SPaT message information shall include timestamp, cycle length, phase sequence, phase durations, +# current phase ID, and the remaining of the current phase durantion. +# +# Expected: True +# TODO implement +########################################################################################################### +''' +Required topics +- SPaT Data : /message/incoming_spat cav_msgs/SPAT +Other Required information +- Movement Ids from the Spat that are needed for the vehicle and scenario being tested (may need to be a hard coded mapping) +- Intersection Id for the spat. Since we have two intersections but only one is used in the test the id of interest should be hard coded into the analysis + With the expected id we can verify that the required correct spat message is received. +Approach: +- Spat will provide the current light phase state by matching the movement id for the current scenario into the plan +- Spat fields can be checked for availability to ensure the listed fields in the metric are available +- Intersection id of the spat message can be checked to ensure it is the correct message being analyzed +''' + + + +### +# Mike suggested. MAP messages received and processed and they contain intersection ids matching the expected values +# This when combined with above metric confirms all required v2i information is available +# TODO implement if extra time +### +''' +Required topics +- Map Data : /message/incoming_map cav_msgs/Map +Other Required Data: +- Map messages have intersection Ids. Since we have two intersections but only one is used in the test the id of interest should be hard coded into the analysis + With the expected id we can verify that the required map message is received. +''' + + +########################################################################################################### +# TSMO UC2-M1.2 : The communication from infrastructure (RSU) to vehicle (OBU) should be within a certain frequency rates +# +# Expected: 10 Hz ± 1 Hz for SPaT, 1 Hz ± 0.5 Hz for MA +# TODO implement +########################################################################################################### +''' +Required topics +- SPaT Data : /message/incoming_spat cav_msgs/SPAT +- Map Data : /message/incoming_map cav_msgs/Map +- Engagement status : /guidance/state cav_msgs/GuidanceState + +Approach: +- After the first 3 Spat messages have been received consider communication stable enough for analysis (this may be before or after engagement) +- Count the number of both spat and map starting from engagement to disengagement. + Divide this number by the total time period of engagement to get total average frequency for each message +- Additionally, create a 1s sliding window for spat and a 5s sliding window for map and verify that the average frequency within these windows never falls outside the specified ranges + +Plotting: +- In addition to the pass/fail criterion plots should be generated of the frequency in the sliding windows at each timestep +''' + +########################################################################################################### +# TSMO UC2-M2.1 : Each vehicle should travel sequentially through the lanelets defined in its path. +# +# Expected: True +# TODO implement. Note the expected set of lanelets changes based on the approach being used by the vehicle +########################################################################################################### + +''' +Required topics +- Current lanelet data : /guidance/route_state cav_msgs/RouteState +- Engagement status : /guidance/state cav_msgs/GuidanceState + +Approach: +- Hard code the expected lanelet ids for each approach in each test scenario +- For a given run after the system is engaged, verify that the route_state reports the current lanelets in sequence following the same order as specified for the scenario and approach +''' + +########################################################################################################### +# +# NOTE: This metric may get tweaked. Suggest tackling last + +# TSMO UC2-M2.2 : Each vehicle shall process the SPaT message +# and adjust its speed such that it can enter the intersection box at a green phase with minimum or no stopping +# Specifically the vehicle should on average stop for less time then would be required by the baseline case of EET (Earliest entry time) +# +# Expected: Vehicle's actual stopping time is less then predicted from EET +# +# TODO implement +########################################################################################################### + +''' +Required topics +- Not a topic, but will need knowledge of current test case being evaluated to determine expected behavior. +- Current lanelet and downtrack: /guidance/route_state cav_msgs/RouteState +- Engagement status : /guidance/state cav_msgs/GuidanceState +- Current_speed : /hardware_interface/vehicle/twist geometry_msgs/TwistStamped (http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TwistStamped.html) +- SPaT Data : /message/incoming_spat cav_msgs/SPAT + +Other Required Data: +- Log file from lci_strategic_plugin with DEBUG logs enabled. This is needed to get the EET value +- Intersection and movement ids for identifying correct spat + +Approach: +- At the start of vehicle engagement look in the lci_strategic_plugin log file for the first occurrence of "earliest_entry_time" This will be the EET value. (from ROS_DEBUG_STREAM("earliest_entry_time: " << std::to_string(earliest_entry_time.toSec()) << ", with : " << earliest_entry_time - current_state.stamp << " left at: " << std::to_string(current_state.stamp.toSec()));) +- Using the EET value determine if the light was red, yellow, or green from SPAT +- If the light was red at EET then find the next time when the light turns green +-- Save the delta between next green start and EET (will refer to as BaseCaseStopTime) +-- Based on the data determine how much time the vehicle was at a stop at the stop bar while engaged (will refer to as VehicleStopTime) +-- If VehicleStopTime < BaseCaseStopTime +---- Record a Successes (algorithm performs better than base case) +- If the light was green at EET +-- If the vehicle stops in this case, +---- Record failure +-- else +---- Record success +- If the light was yellow at EET +-- If the vehicle stops +---- Record success +-- If the vehicle does not stop +---- Record failure + + +''' + +########################################################################################################### +# TSMO UC2-M2.3 : Vehicles shall not enter the intersection box at yellow or red phases. +# +# Expected: True +# TODO implement +########################################################################################################### + +''' +Required topics +- Current lanelet data : /guidance/route_state cav_msgs/RouteState +- Engagement status : /guidance/state cav_msgs/GuidanceState +- SPaT Data : /message/incoming_spat cav_msgs/SPAT +Other Required Data: +- The intersection and movement ids expected for the vehicle and scenario being tested + +Approach: +- Use the intersection id and movement id to identify the current phase of the light which is required +- Use the route_state to determine the current lanelet of the vehicle +- While the vehicle is engaged, its reported lanelet should never be one inside the intersection while the phase state is not green +''' + +########################################################################################################### +# TSMO UC2-M2.4 : Each vehicle should switch away from the TS mode after entering the intersection box +# Note that the intersection box is the box per the HD map, which may not be exactly as physically seen on the road. +# Expected: True +# TODO implement +########################################################################################################### + +''' +Required topics +- Current lanelet data : /guidance/route_state cav_msgs/RouteState +- Engagement status : /guidance/state cav_msgs/GuidanceState +- Determine lci plugin is used in planning : /guidance/plan_trajectory cav_msgs/TrajectoryPlan + +Approach: +- When the vehicle is engaged, gets its current lanelet from the route state +- Verify that when the lanelet is one inside the intersection the current planning trajectory is not coming from the lci tactical plugin +- To check the controlling tactical plugin look at the planner_plugin field of the first point on the current plan_trajectory +''' + +########################################################################################################### +# TSMO UC2-M2.3 : The average deceleration\acceleration over any 1-second portion of the test time horizon +# shall be no greater than 3.0 m/s^2. +# +# Expected: acceleration <= 3.0 m/s^2 deceleration >= -3.0 m/s^2 +# TODO implement +########################################################################################################### + +''' +Required topics +- Engagement status : /guidance/state cav_msgs/GuidanceState +- Current_speed : /hardware_interface/vehicle/twist (use to compute accel) geometry_msgs/TwistStamped +- Vehicle accel : /hardware_interface/velocity_accel_cov (use as alternative source of accel) automotive_platform_msgs/VelocityAccelCov (https://github.com/astuff/automotive_autonomy_msgs/blob/master/automotive_platform_msgs/msg/VelocityAccelCov.msg) + +Approach: +- While the vehicle is engaged, create a 1s sliding window and take the start and end speed for each timestep of the window and compute the average acceleration +- Additionally, verify that the abs(/hardware_interface/velocity_accel_cov reported value) never exceeds 3.0 for more than 1 timestep at any point during engagement +''' + +### +# Mike suggested. Vehicle does not violate speed limit my more than 1 mph +# Good to confirm as this showed up as an issue multiple times during integration testing +# TODO implement if extra time +### +''' +Required topics +- Engagement status : /guidance/state cav_msgs/GuidanceState +- Current_speed : /hardware_interface/vehicle/twist geometry_msgs/TwistStamped +Other Required Data: +- Speed limit extracted from map + +Approach: +- In TSMO UC2 all the speed limits will be the same so a simple conditional while the vehicle is engaged on twist.linear.x will suffice to check if the limit is exceeded +''' + + +########################################################################################################### +# Additional Requested Plots: Speed over time with matching signal state shown +########################################################################################################### + +''' +Required topics +- Engagement status : /guidance/state +- Current_speed : /hardware_interface/vehicle/twist +- SPaT Data : /message/incoming_spat + +Other Required Data: +- Intersection id and movement id for the spat message + +Approach: +- While the vehicle is engaged plot the current speed twist.linear.x +- Also plot the signal phase as red, green, yellow bars on the same graph + +NOTE: This plot should be generated in two forms 1 time for the individual vehicles and once as a single plot combing the data from two vehicles +''' + +########################################################################################################### +# Additional Requested Plots: Downtrack position over time with matching signal state shown +########################################################################################################### + +''' +Required topics +- Engagement status : /guidance/state +- Current downtrack data : /guidance/route_state +- SPaT Data : /message/incoming_spat +Other Required Data: +- Intersection id and movement id for the spat message + +Approach: +- While the vehicle is engaged plot the current downtrack distance reported by route_state +- Identify where the stop bar is in terms of downtrack distance (this might change per run) +- At the position where the stop bar is (y-axis) draw a set of red/green/yellow rectangles showing the current signal phase + +NOTE: This plot should be generated in two forms 1 time for the individual vehicles and once as a single plot combing the data from two vehicles +''' + +########################################################################################################### +# Additional Requested Plots: Acceleration over time with matching signal state shown +########################################################################################################### + +''' +Required topics +- Engagement status : /guidance/state +- Current_speed : /hardware_interface/vehicle/twist (use to compute accel) +- Vehicle accel : /hardware_interface/velocity_accel_cov (use as alternative source of accel) +- SPaT Data : /message/incoming_spat +Other Required Data: +- Intersection id and movement id for the spat message + +Approach: +- While the vehicle is engaged plot the current acceleration +- Also plot the signal phase as red, green, yellow bars on the same graph + +NOTE: This plot should be generated in two forms 1 time for the individual vehicles and once as a single plot combing the data from two vehicles +''' + + +# Main Function; run all tests from here +# TODO: The contents of this main function provide some basic structure for loading data, but need not be followed if not applicable +def main(): + if len(sys.argv) < 2: + print("Need 1 arguments: process_bag.py ") + exit() + + source_folder = sys.argv[1] + + # Re-direct the output of print() to a specified .txt file: + orig_stdout = sys.stdout + current_time = datetime.datetime.now() + text_log_filename = "Results_" + str(current_time) + ".txt" + text_log_file_writer = open(text_log_filename, 'w') + sys.stdout = text_log_file_writer + + # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): + csv_results_filename = "Results_" + str(current_time) + ".csv" + csv_results_writer = csv.writer(open(csv_results_filename, 'w')) + + # TODO update this list + csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", + "WZ-1 Result", "WZ-2 Result", "WZ-3 Result", "WZ-4 Result", "WZ-5 Result", "WZ-6 Result", + "WZ-7 Result", "WZ-8 Result", "WZ-9 Result", "WZ-10 Result","WZ-11 Result", "WZ-12 Result", + "WZ-13 Result", "WZ-14 Result", "WZ-15 Result", "WZ-16 Result", "WZ-17 Result", "WZ-18 Result", + "WZ-19 Result", "WZ-20 Result", "WZ-21 Result", "WZ-22 Result", "WZ-23 Result", "WZ-24 Result", "WZ-25 Result"]) + + # TODO identify cases and assign bags to each case + # Create list of Red Light Workzone Black Pacifica bag files to be processed + black_pacifica_red_bag_files = [] + + # Create list of Red Light Workzone Ford Fusion bag files to be processed + ford_fusion_red_bag_files = ["_2021-09-22-16-00-28-red.bag", + "_2021-09-22-19-03-26-red.bag"] + + # Create list of Red Light Workzone Blue Lexus bag files to be processed + blue_lexus_red_bag_files = [] + + # Create list of Green Light Workzone Black Pacifica bag files to be processed + black_pacifica_green_bag_files = [] + + # Create list of Green Light Workzone Ford Fusion bag files to be processed + ford_fusion_green_bag_files = [] + + # Create list of Green Light Workzone Blue Lexus bag files to be processed + blue_lexus_green_bag_files = [] + + # Concatenate all Basic Travel bag files into one list + red_light_bag_files = black_pacifica_red_bag_files + ford_fusion_red_bag_files + blue_lexus_red_bag_files + green_light_bag_files = black_pacifica_green_bag_files + ford_fusion_green_bag_files + blue_lexus_green_bag_files + WZ_bag_files = red_light_bag_files + green_light_bag_files + + # Loop to conduct data anlaysis on each bag file: + for bag_file in WZ_bag_files: + print("*****************************************************************") + print("Processing new bag: " + str(bag_file)) + # TODO update bags groups + if bag_file in black_pacifica_red_bag_files: + print("Black Pacifica Red Light Workzone Test Case") # TODO update conditionals here + elif bag_file in ford_fusion_red_bag_files: + print("Ford Fusion Red Light Workzone Test Case") + elif bag_file in blue_lexus_red_bag_files: + print("Blue Lexus Red Light Workzone Test Case") + if bag_file in black_pacifica_green_bag_files: + print("Black Pacifica Green Light Workzone Test Case") + elif bag_file in ford_fusion_green_bag_files: + print("Ford Fusion Green Light Workzone Test Case") + elif bag_file in blue_lexus_green_bag_files: + print("Blue Lexus Green Light Workzone Test Case") + + # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): + sys.stdout = orig_stdout + print("Processing bag file " + str(bag_file) + " (" + str(WZ_bag_files.index(bag_file) + 1) + " of " + str(len(WZ_bag_files)) + ")") + sys.stdout = text_log_file_writer + + # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file + try: + print("Starting To Process Bag at " + str(datetime.datetime.now())) + bag_file_path = str(source_folder) + "/" + bag_file + bag = rosbag.Bag(bag_file_path) + print("Finished Processing Bag at " + str(datetime.datetime.now())) + except: + print("Skipping " + str(bag_file) +", unable to open or process bag file.") + continue + + + # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test + # TODO update to remove geofence information + print("Getting engagement times at " + str(datetime.datetime.now())) + # TODO get engagement times + #time_test_start_engagement, time_test_end_engagement, found_test_times = get_test_case_engagement_times(bag) + print("Got engagement times at " + str(datetime.datetime.now())) + found_test_times = True # TODO remove these placeholder values + time_test_start_engagement = 0.0 + time_test_end_engagement = 1.0 + if (not found_test_times): + print("Could not find test case engagement start and end times in bag file.") + continue + + # Debug Statements + print("Engagement starts at " + str(time_test_start_engagement.to_sec())) + print("Engagement ends at " + str(time_test_end_engagement.to_sec())) + print("Time spent engaged: " + str((time_test_end_engagement - time_test_start_engagement).to_sec()) + " seconds") + + # TODO implement + original_speed_limit = 1.0 # TODO remove placeholder + # original_speed_limit = get_route_original_speed_limit(bag, time_test_start_engagement) # Units: m/s + print("Original Speed Limit is " + str(original_speed_limit) + " m/s") + + # Initialize results + # TODO updates results + wz_1_result = None + wz_2_result = None + wz_3_result = None + wz_4_result = None + wz_5_result = None + wz_6_result = None + wz_7_result = None + wz_8_result = None + wz_9_result = None + wz_10_result = None + wz_11_result = None + wz_12_result = None + wz_13_result = None + wz_14_result = None + wz_15_result = None + wz_16_result = None + wz_17_result = None + wz_18_result = None + wz_19_result = None + wz_20_result = None + wz_21_result = None + wz_22_result = None + wz_23_result = None + wz_24_result = None + wz_25_result = None + wz_26_result = None + wz_27_result = None + + # TODO call metric functions + + # Write simple pass/fail results to .csv file for appropriate row: + csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, + wz_1_result, wz_2_result, wz_3_result, wz_4_result, wz_5_result, wz_6_result, wz_7_result, + wz_8_result, wz_9_result, wz_10_result, wz_11_result, wz_12_result, wz_13_result, wz_14_result, + wz_15_result, wz_16_result, wz_17_result, wz_18_result, wz_19_result, wz_20_result, + wz_21_result, wz_22_result, wz_23_result, wz_24_result, wz_25_result]) + + sys.stdout = orig_stdout + text_log_file_writer.close() + return + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_voices_sit1_rosbags.py b/engineering_tools/data_processing/analyze_voices_sit1_rosbags.py new file mode 100644 index 0000000000..3ac1f8b893 --- /dev/null +++ b/engineering_tools/data_processing/analyze_voices_sit1_rosbags.py @@ -0,0 +1,776 @@ +#!/usr/bin/python3 + +# 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. + +from inspect import TPFLAGS_IS_ABSTRACT +import sys +import csv +import matplotlib.pyplot as plt +import rospy +import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" +import datetime +import math + +# HOW TO USE SCRIPT: +# Run the following in a terminal: +# sudo add-apt-repository ppa:deadsnakes/ppa +# sudo apt-get update +# sudo apt install python3.7 +# python3.7 -m pip install --upgrade pip +# python3.7 -m pip install matplotlib +# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg +# python3.7 -m pip install lz4 +# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ +# In terminal, navigate to the directory that contains this python script and run the following: +# python3.7 analyze_voices_sit1_rosbags.py + +def generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_starting_vehicle_start_engagement, vehicle_1_platooning_times, vehicle_2_platooning_times, vehicle_3_platooning_times, test_type, test_num): + # vehicle 2 and 3 platooning times: [time_veh1_engaged, time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] + # vehicle 1 platooning times: [time_began_platooning_with_veh2, time_began_platooning_with_veh3, time_leader_end_engagement] + + if test_type == "VOICES-SIT1": + # First vehicle to engage is vehicle 1 + + # Get the true vehicle speed (m/s) and the associated time with each data point + first = True + vehicle_1_speed_times = [] + vehicle_1_speeds = [] + for topic, msg, t in vehicle_1_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = vehicle_1_platooning_times[2]): # time_start_engagement+time_duration): + if first: + time_1_start = t + first = False + continue + + vehicle_1_speed_times.append((t-time_1_start).to_sec()) + vehicle_1_speeds.append(msg.twist.linear.x) # Current speed in m/s + + first = True + vehicle_2_speed_times = [] + vehicle_2_speeds = [] + for topic, msg, t in vehicle_2_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = vehicle_2_platooning_times[0], end_time = vehicle_2_platooning_times[3]): # time_start_engagement+time_duration): + if first: + time_2_start = t + first = False + continue + + vehicle_2_speed_times.append((t-time_2_start).to_sec()) + vehicle_2_speeds.append(msg.twist.linear.x) # Current speed in m/s + + first = True + vehicle_3_speed_times = [] + vehicle_3_speeds = [] + for topic, msg, t in vehicle_3_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = vehicle_3_platooning_times[0], end_time = vehicle_3_platooning_times[3]): # time_start_engagement+time_duration): + if first: + time_3_start = t + first = False + continue + + vehicle_3_speed_times.append((t-time_3_start).to_sec()) + vehicle_3_speeds.append(msg.twist.linear.x) # Current speed in m/s + + # Create the initial plot with the defined figure size + fig, ax = plt.subplots(figsize=(9,5.5)) + + # Plot vehicle 1 speed (m/s) vs. time + ax.plot(vehicle_1_speed_times, vehicle_1_speeds, 'g:', label='Vehicle 1 Speed (m/s)') + + # Plot vehicle 2 speed (m/s) vs. time + ax.plot(vehicle_2_speed_times, vehicle_2_speeds, 'b:', label='Vehicle 2 Speed (m/s)') + + # Plot vehicle 2 speed (m/s) vs. time + ax.plot(vehicle_3_speed_times, vehicle_3_speeds, 'r:', label='Vehicle 3 Speed (m/s)') + + # Optional: Plot a vertical bar at the time that vehicle 2 joins the platoon + #ax.axvline(x = (vehicle_2_platooning_times[2] - time_2_start).to_sec(), color = 'b', label = 'Time Vehicle 2 Joined the Platoon') + + # Optional: Plot a vertical bar at the time that vehicle 3 joins the platoon + #ax.axvline(x = (vehicle_3_platooning_times[2] - time_3_start).to_sec(), color = 'r', label = 'Time Vehicle 3 Joined the Platoon') + + plt.rc('axes', labelsize=12) # fontsize of the axes labels + plt.rc('legend', fontsize=10) # fontsize of the legend text + ax.legend(loc = 'lower right') # Location of the legend + ax.set_title(str(test_type) + " Run " + str(test_num) + " Vehicle Speeds") # Plot Title + ax.set_xlabel("Time (seconds) Since Lead Vehicle Became Engaged") # Plot X Title + ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title + + # Option 1: Save the plot + filename = str(test_type) + "_Run_" + str(test_num) + "_Vehicle_Speeds.png" + plt.savefig(filename, bbox_inches='tight') + plt.close() + + # Option 2: Display the plot + #plt.show() + + return + +def generate_speed_plot(bag, time_start_engagement, time_end_engagement, bag_file_name, speed_limit_zone_change_times, test_type, vehicle_number): + # Speed command: /hardware_interface/arbitrated_speed_commands: msg.speed (m/s) + # True Speed: /hardware_interface/vehicle/twist: msg.twist.linear.x (m/s) + + # Get the true vehicle speed (m/s) and the associated time with each data point + first = True + true_vehicle_speed_times = [] + true_vehicle_speeds = [] + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): + if first: + time_start = t + first = False + continue + + true_vehicle_speed_times.append((t-time_start).to_sec()) + true_vehicle_speeds.append(msg.twist.linear.x) # Current speed in m/s + + # TODO: UPDATE THE TOPIC USED FOR THE LEXUS/PACMOD SPEED COMMANDS + # Get the commanded vehicle speed (m/s) and the associated time with each data point + first = True + cmd_vehicle_speed_times = [] + cmd_vehicle_speeds = [] + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/arbitrated_speed_commands'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): + if first: + time_start = t + first = False + continue + + cmd_vehicle_speed_times.append((t-time_start).to_sec()) + cmd_vehicle_speeds.append(msg.speed) # Commanded speed in m/s + + # Create the initial plot with the defined figure size + fig, ax = plt.subplots(figsize=(9,5.5)) + + # Plot commanded vehicle speed (m/s) vs. time + ax.plot(cmd_vehicle_speed_times, cmd_vehicle_speeds, 'g:', label='Commanded Speed (m/s)') + + # Plot true vehicle speed (m/s) vs. time + ax.plot(true_vehicle_speed_times, true_vehicle_speeds, 'b--', label='Actual Speed (m/s)') + + plt.rc('axes', labelsize=12) # fontsize of the axes labels + plt.rc('legend', fontsize=10) # fontsize of the legend text + ax.legend(loc = 'lower right') # Location of the legend + ax.set_title(str(bag_file_name) + " Speed (Commanded and Actual) -- Vehicle " + str(vehicle_number)) # Plot Title + ax.set_xlabel("Time (seconds) Since Start of Engagement") # Plot X Title + ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title + + # Option 1: Save the plot + filename = str(test_type) + "_" + bag_file_name + ".png" + plt.savefig(filename, bbox_inches='tight') + plt.close() + + # Option 2: Display the plot + #plt.show() + + return + +def generate_platooning_plot(bag, time_start_platooning, time_end_platooning, bag_file_name, test_type, vehicle_number): + + actual_gaps = [] + desired_gaps = [] + times = [] + first = True + for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_start_platooning, end_time = time_end_platooning): + if first: + time_start = t + first = False + continue + + if msg.actual_gap < 150 and msg.actual_gap > -150: + actual_gaps.append(msg.actual_gap) + desired_gaps.append(msg.desired_gap) + times.append((t-time_start).to_sec()) + + # Create the initial plot with the defined figure size + fig, ax = plt.subplots(figsize=(9,5.5)) + + # Plot actual gaps (meters) vs. time + ax.plot(times, actual_gaps, 'g:', label='Actual Gap (meters)') + + # Plot desired gaps (meters) vs. time + ax.plot(times, desired_gaps, 'r--', label='Desired Gap (meters)') + + plt.rc('axes', labelsize=12) # fontsize of the axes labels + plt.rc('legend', fontsize=10) # fontsize of the legend text + ax.legend(loc = 'lower right') # Location of the legend + ax.set_title(str(bag_file_name) + " Platooning Information -- Vehicle " + str(vehicle_number)) # Plot Title + ax.set_xlabel("Time (seconds) Since Joining Platoon") # Plot X Title + ax.set_ylabel("Meters") # Plot Y Title + + # Option 1: Save the plot + filename = str(test_type) + "_Platooning_Information_" + bag_file_name + ".png" + plt.savefig(filename, bbox_inches='tight') + plt.close() + + # Option 2: Display the plot + #plt.show() + + return + +def generate_crosstrack_plot(bag, time_start_engagement, time_end_engagement, bag_file_name): + # Crosstrack Error: /message/route_state (meters)) + + # Get the true vehicle speed (m/s) and the associated time with each data point + first = True + crosstrack_times = [] + crosstrack_values = [] + # Note: This topic name assumes a pacmod controller is being used (freightliners or lexus) + for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_end_engagement): + if first: + time_start = t + first = False + continue + + crosstrack_times.append((t-time_start).to_sec()) + crosstrack_values.append(msg.cross_track) # Crosstrack error in meters + + # Create the initial plot with the defined figure size + fig, ax = plt.subplots(figsize=(9,5.5)) + + # Plot crosstrack error (meters) vs. time + ax.plot(crosstrack_times, crosstrack_values, 'g:', label='Crosstrack Error (meters)') + + # Optional: Plot a horizontal bar at a positive value for reference + ax.axhline(y = 0.95, color = 'r', label = '+/- 0.95 Meters (For Reference)') + + # Optional: Plot a horizontal bar at a negative value for reference + ax.axhline(y = -0.95, color = 'r') + + plt.rc('axes', labelsize=12) # fontsize of the axes labels + plt.rc('legend', fontsize=10) # fontsize of the legend text + ax.legend(loc = 'lower right') # Location of the legend + ax.set_title(str(bag_file_name) + " Crosstrack Error") # Plot Title + ax.set_xlabel("Time (seconds) Since Start of Engagement") # Plot X Title + ax.set_ylabel("Crosstrack Error (meters)") # Plot Y Title + + # Option 1: Save the plot + filename = "Crosstrack_" + bag_file_name + ".png" + #plt.savefig(filename, bbox_inches='tight') + #plt.close() + + # Option 2: Display the plot + plt.show() + + return + +# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section +def get_test_case_engagement_times(bag): + # Initialize system engagement start and end times + time_start_engagement = rospy.Time() + time_stop_engagement = rospy.Time() + + # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section + is_engaged = False + found_starting_engagement_time = False + found_ending_engagement_time = False + for topic, msg, t in bag.read_messages(topics=['/guidance/state']): + # If entering engagement, track this start time + if (msg.state == 4 and not is_engaged): + found_starting_engagement_time = True + time_start_engagement = t + is_engaged = True + + # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance + # state can be published. + if (msg.state == 4): + time_last_engaged = t + + # Log time that engagements ends + elif (msg.state != 4 and is_engaged): + found_ending_engagement_time = True + is_engaged = False + time_stop_engagement = t + + # If CARMA ended engagement before guidance state could be updated, update time_stop_engagement + if found_starting_engagement_time and not found_ending_engagement_time: + time_stop_engagement = time_last_engaged + + found_engagement_times = False + if found_starting_engagement_time: + found_engagement_times = True + + return time_start_engagement, time_stop_engagement, found_engagement_times + +def analyze_route_speed_limits(bag): + print("New route analyzed: ") + current_lanelet = None + current_speed_limit = None + lanelet_speed_limits = [] + speed_limit_zone_change_times = [] + speed_limit_zone_values = [] + for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): + if msg.lanelet_id != current_lanelet: + lanelet_speed_limits.append(msg.speed_limit) + current_lanelet = msg.lanelet_id + + if current_speed_limit is None: + print("New speed limit " + str(msg.speed_limit) + " at time " + str(t.to_sec())) + speed_limit_zone_values.append(msg.speed_limit) + speed_limit_zone_change_times.append(t) + current_speed_limit = msg.speed_limit + else: + if abs(current_speed_limit - msg.speed_limit) > 0.1: + print("New speed limit " + str(msg.speed_limit) + " at time " + str(t.to_sec())) + speed_limit_zone_values.append(msg.speed_limit) + speed_limit_zone_change_times.append(t) + current_speed_limit = msg.speed_limit + + # Remove first element for zero speed limit + lanelet_speed_limits = lanelet_speed_limits[1:] + speed_limit_zone_change_times = speed_limit_zone_change_times[1:] + speed_limit_zone_values = speed_limit_zone_values[1:] + + print("Route speed limits in m/s (1 entry for each shortest-path lanelet in order): " + str(lanelet_speed_limits[1:])) + + return speed_limit_zone_values,speed_limit_zone_change_times + + +def leader_platoon_formation_analysis(bag, run_type, leader_id, second_vehicle_id, third_vehicle_id, time_leader_end_engagement): + + if run_type == "VOICES-SIT1": + has_began_platooning_with_veh2 = False + time_began_platooning_with_veh2 = rospy.Time() + has_began_platooning_with_veh3 = False + time_began_platooning_with_veh3 = rospy.Time() + ack_num = 0 + for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_response']): + if msg.header.sender_id == leader_id and msg.header.recipient_id == second_vehicle_id: + if msg.is_accepted: + ack_num += 1 + + if ack_num == 2: + has_began_platooning_with_veh2 = True + time_began_platooning_with_veh2 = t + print("Leader began platooning with veh2 at " + str(t.to_sec())) + elif msg.header.sender_id == leader_id and msg.header.recipient_id == third_vehicle_id: + if msg.is_accepted: + ack_num += 1 + + if ack_num == 4: + has_began_platooning_with_veh3 = True + time_began_platooning_with_veh3 = t + print("Leader began platooning with veh3 at " + str(t.to_sec())) + break + + + return [time_began_platooning_with_veh2, time_began_platooning_with_veh3, time_leader_end_engagement] + +def joiner_platoon_formation_analysis(bag, run_type, leader_id, follower_id, joiner_vehicle_number): + + has_sent_initial_request = False + time_sent_initial_request = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request']): + if msg.header.recipient_id == leader_id and msg.header.sender_id == follower_id: + if run_type == "VOICES-SIT1": + if msg.plan_type.type == 3: + has_sent_initial_request = True + time_sent_initial_request = t + print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " sent initial request at " + str(t.to_sec())) + + has_received_initial_ack = False + time_received_initial_ack = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response']): + if msg.header.recipient_id == follower_id and msg.header.sender_id == leader_id: + if run_type == "VOICES-SIT1": + #if msg.plan_type.type == 3 and msg.is_accepted: + if msg.is_accepted: + has_received_initial_ack = True + time_received_initial_ack = t + print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " received ACK at " + str(t.to_sec())) + break + + has_sent_second_request = False + time_sent_second_request = rospy.Time() + first = True + for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request']): + if msg.header.recipient_id == leader_id and msg.header.sender_id == follower_id: + if run_type == "VOICES-SIT1": + if msg.plan_type.type == 4: + if first: + has_sent_second_request = True + time_sent_second_request = t + first = False + print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " sent second request at " + str(t.to_sec())) + + has_received_second_ack = False + time_received_second_ack = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response']): + if msg.header.recipient_id == follower_id and msg.header.sender_id == leader_id: + if run_type == "VOICES-SIT1": + if msg.is_accepted: + has_received_second_ack = True + time_received_second_ack = t + print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " received ACK at " + str(t.to_sec())) + + total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() + #print("DEBUG: Time between sending initial request and receiving ack: " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") + #print("DEBUG: Time between sending second request and receiving ack: " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec") + #print("DEBUG: Time between sending initial request and receiving second ack: " + str(total_formation_duration) + " sec") + + if run_type == "VOICES-SIT1": + total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() + if joiner_vehicle_number == 2: + if has_received_initial_ack: + print("IHP2-1 Succeeded - Vehicle 2 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") + else: + print("IHP2-1 Failed - Vehicle 2 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") + + if has_received_second_ack: + print("IHP2-3 Succeeded - Vehicle 2 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") + else: + print("IHP2-3 Failed - Vehicle 2 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") + + if total_formation_duration <= 2.0: + print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") + else: + print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") + + return [time_received_initial_ack, time_received_second_ack] + + elif joiner_vehicle_number == 3: + if has_received_initial_ack: + print("IHP2-7 Succeeded - Vehicle 3 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") + else: + print("IHP2-7 Failed - Vehicle 3 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") + + if has_received_second_ack: + print("IHP2-9 Succeeded - Vehicle 3 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") + else: + print("IHP2-9 Failed - Vehicle 3 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") + + if total_formation_duration <= 2.0: + print("IHP2-8 Succeeded: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") + else: + print("IHP2-8 Failed: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") + + return [time_received_initial_ack, time_received_second_ack] + +def received_tcm_analysis(bag, time_start_platooning, test_type, vehicle_number, expected_tcm_count): + + # List to store the receive times of each TCM msgnum [time_receive_msgnum1, time_receive_msgnum2, etc.] + tcm_receive_times = [None for i in range(0, expected_tcm_count)] + + # Store all transmitted TCR reqids so that only corresponding received TCMs are analyzed + tcr_reqids = [] + tcr_reqids_times = [] + for topic, msg, t in bag.read_messages(topics=['/message/outgoing_geofence_request']): + tcr_reqids.append(msg.tcr_v01.reqid.id) + tcr_reqids_times.append(t) + + # Only analyze TCMs with reqids matching a transmitted TCR reqid. Populate the tcm_receive_times list + has_received_any_tcms = False + has_received_all_tcms = False + has_received_correct_tcms = True + for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): + reqid = msg.tcm_v01.reqid.id + if reqid in tcr_reqids: + has_received_any_tcms = True + msgnum = int(msg.tcm_v01.msgnum) - 1 + if tcm_receive_times[msgnum] is None: + time_received_after_platoon_start = (t-time_start_platooning).to_sec() + tcm_receive_times[msgnum] = time_received_after_platoon_start + + tcm_type = msg.tcm_v01.params.detail.choice + + if tcm_type == 12: + max_speed = msg.tcm_v01.params.detail.maxspeed + print("DEBUG: Received TCM msgnum " + str(msgnum) + " of " + str(msg.tcm_v01.msgtot) + " with type " + str(tcm_type) + " " + str(time_received_after_platoon_start) + " sec after platooning -- max speed " + str(max_speed)) + else: + print("Received non-maxspeed TCM") + + if None not in tcm_receive_times: + has_received_all_tcms = True + break + + if not has_received_any_tcms: + print("IHP2-4 Failed for vehicle " + str(vehicle_number) + ": No TCMs were received matching its TCR reqids") + print("IHP2-5 Failed for vehicle " + str(vehicle_number) + ": No TCMs were received matching its TCR reqids") + return + + #if has_received_all_tcms: + # time_received_first_tcm = min(tcm_receive_times) + # print("DEBUG: Vehicle " + str(vehicle_number) + " received first applicable TCM " + str(time_received_first_tcm) + " sec after beginning platooning") + + if min(tcm_receive_times) > 0: + print("IHP2-4 Succeeded for vehicle " + str(vehicle_number) + ": all TCMs were received after the vehicle began platooning") + else: + print("IHP2-4 Failed for vehicle " + str(vehicle_number) + ": at least one TCM was received before vehicle began platooning") + + if has_received_all_tcms and has_received_correct_tcms: + print("IHP2-5 Succeeded: CMV received all correct TCMs for test type " + str(test_type)) + elif not has_received_correct_tcms and not has_received_correct_tcms: + print("IHP2-5 Failed: CMV didn't receive all TCMs and didn't receive correct TCMs for test type " + str(test_type)) + elif not has_received_correct_tcms: + print("IHP2-5 Failed: CMV did not receive all correct TCMs for test type " + str(test_type)) + elif not has_received_all_tcms: + print("IHP2-5 Failed: CMV did not receive all TCM msgnums for test type " + str(test_type)) + + +def crosstrack_distance_analysis(join_vehicle_rosbag, other_vehicle_rosbag, run_type, join_vehicle_number, other_vehicle_number, time): + # Get other vehicle cross track at provided time (negative is to the left; positive is to the right) + for topic, msg, t in other_vehicle_rosbag.read_messages(topics=['/guidance/route_state'], start_time = time): + other_vehicle_cross_track = msg.cross_track + break + + # Get joining vehicle pose at provided time (negative is to the left; positive is to the right) + for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/guidance/route_state'], start_time = time): + join_vehicle_cross_track = msg.cross_track + break + + total_cross_track = abs(other_vehicle_cross_track - join_vehicle_cross_track) + print("DEBUG: Join vehicle " + str(join_vehicle_number) + " crosstrack " + str(join_vehicle_cross_track) + ", veh " + str(other_vehicle_number) + " crosstrack " + str(other_vehicle_cross_track) + ", total " + str(total_cross_track) + " meters at time " + str(time.to_sec())) + + if run_type == "ALR": + if total_cross_track < 1.5: + print("IHP2-20 Succeeded: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FOLLOWER_JOIN") + else: + print("IHP2-20 Failed: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FOLLOWER_JOIN") + elif run_type == "ALF": + if total_cross_track < 1.5: + print("IHP2-22 Succeeded: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FRONT_JOIN") + else: + print("IHP2-22 Failed: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FRONT_JOIN") + + return total_cross_track + +def downtrack_distance_analysis(join_vehicle_rosbag, other_vehicle_rosbag, run_type, join_vehicle_number, other_vehicle_number, time): + # Get other vehicle pose at provided time + for topic, msg, t in other_vehicle_rosbag.read_messages(topics=['/localization/current_pose'], start_time = time): + other_vehicle_pose = [msg.pose.position.x, msg.pose.position.y] + break + + # Get joining vehicle pose at provided time + for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/localization/current_pose'], start_time = time): + join_vehicle_pose = [msg.pose.position.x, msg.pose.position.y] + break + + # Get joining vehicle speed + for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time): + join_vehicle_speed = msg.twist.linear.x + break + + distance_between_vehicles = ((other_vehicle_pose[0] - join_vehicle_pose[0])**2 + (other_vehicle_pose[1] - join_vehicle_pose[1])**2) ** 0.5 + time_gap_between_vehicles = distance_between_vehicles / join_vehicle_speed + + print("DEBUG: Joining vehicle " + str(join_vehicle_number) + " speed is " + str(join_vehicle_speed) + " m/s") + print("DEBUG: Distance between joining vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " at time " + str(time.to_sec()) + " is " + str(distance_between_vehicles) + " meters") + print("DEBUG: Time Gap between joining vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " at time " + str(time.to_sec()) + " is " + str(time_gap_between_vehicles) + " seconds") + + if run_type == "VOICES-SIT1": + # Values are the same for same-lane front join and same-lane rear join + if distance_between_vehicles <= 90.0 or time_gap_between_vehicles <= 15.0: + print("IHP2-17 (SLR) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") + else: + print("IHP2-17 (SLR) Failed: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") + + return distance_between_vehicles + +# Main Function; run all tests from here +def main(): + if len(sys.argv) < 2: + print("Need 1 arguments: process_bag.py ") + exit() + + source_folder = sys.argv[1] + + # # Re-direct the output of print() to a specified .txt file: + # orig_stdout = sys.stdout + # current_time = datetime.datetime.now() + # text_log_filename = "Results_" + str(current_time) + ".txt" + # text_log_file_writer = open(text_log_filename, 'w') + # sys.stdout = text_log_file_writer + + # # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): + # csv_results_filename = "Results_" + str(current_time) + ".csv" + # csv_results_writer = csv.writer(open(csv_results_filename, 'w')) + # csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", + # "IHP2-1", "IHP2-2", "IHP2-3", "IHP2-4", "IHP2-5", + # "IHP2-6", "IHP2-7", "IHP2-8", "IHP2-9", "IHP2-10", + # "IHP2-11", "IHP2-12", "IHP2-13", "IHP2-14", "IHP2-15", + # "IHP2-16", "IHP2-17", "IHP2-18", "IHP2-19"]) + + # Constants + SILVER_LEXUS = "DOT-45243" + SPR = "CARMA-SPR" + AUG = "CARMA-AUG" + + bag_file = "_2022-07-28-18-35-40.bag" + bag_file_path = str(source_folder) + "/" + bag_file + bag = rosbag.Bag(bag_file_path) + num = 0 + for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): + print("***********************************************") + print(msg) + num += 1 + if num == 5: + break + + sit1_bag_files = [[["veh1_run2_2022-07-28-16-05-55-run2-veh1.bag", + "veh2_run2_2022-07-28-16-08-05.bag", + "veh3_run2_2022-07-28-16-07-02.bag"], + [SILVER_LEXUS, AUG, SPR]]] + + # Loop to conduct data analysis on each grouping of test run rosbags: + test_num = 0 + for test_case_files in sit1_bag_files: + test_num += 1 + print("*****************************************************************") + if test_case_files in sit1_bag_files: + test_type = "VOICES-SIT1" + print("VOICES - SIT1 Test Case") + else: + print("Unknown test run type being processed.") + continue + + # Store the rosbag and vehicle type for each vehicle in the lineup (Vehicle 1, Vehicle 2, Vehicle 3) + bag_files_list = test_case_files[0] + vehicle_types = test_case_files[1] + for file_number in range(0, len(bag_files_list)): + print("--") + bag_file = bag_files_list[file_number] + if file_number == 0: + vehicle_number = 1 + vehicle_1_rosbag_name = bag_files_list[file_number] + vehicle_1_type = vehicle_types[0] + elif file_number == 1: + vehicle_number = 2 + vehicle_2_rosbag_name = bag_files_list[file_number] + vehicle_2_type = vehicle_types[1] + elif file_number == 2: + vehicle_number = 3 + vehicle_3_rosbag_name = bag_files_list[file_number] + vehicle_3_type = vehicle_types[2] + + print("Processing new bag for vehicle " + str(file_number+1) + ": " + str(bag_file) + " for " + str(vehicle_types[file_number])) + + # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): + # sys.stdout = orig_stdout + print("Processing bag file " + str(bag_file) + " (" + str(sit1_bag_files.index(test_case_files) + 1) + " of " + str(len(sit1_bag_files)) + ")") + # sys.stdout = text_log_file_writer + + # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file + try: + print("Starting To Process Bag at " + str(datetime.datetime.now())) + bag_file_path = str(source_folder) + "/" + bag_file + + if vehicle_number == 1: + vehicle_1_rosbag = rosbag.Bag(bag_file_path) + elif vehicle_number == 2: + vehicle_2_rosbag = rosbag.Bag(bag_file_path) + elif vehicle_number == 3: + vehicle_3_rosbag = rosbag.Bag(bag_file_path) + + print("Finished Processing Bag at " + str(datetime.datetime.now())) + except: + print("Skipping " + str(bag_file) +", unable to open or process bag file.") + continue + + # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test + print("Getting engagement times at " + str(datetime.datetime.now())) + if vehicle_number == 1: + time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_1_rosbag) + time_1_start_engagement = time_start_engagement + time_1_end_engagement = time_end_engagement + if vehicle_number == 2: + time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_2_rosbag) + time_2_start_engagement = time_start_engagement + time_2_end_engagement = time_end_engagement + if vehicle_number == 3: + time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_3_rosbag) + time_3_start_engagement = time_start_engagement + time_3_end_engagement = time_end_engagement + print("Got engagement times at " + str(datetime.datetime.now())) + + if (not found_test_times): + print("Could not find test case engagement start and end times in bag file.") + #continue + else: + print("Began engagement at " + str(time_start_engagement.to_sec())) + print("Ended engagement at " + str(time_end_engagement.to_sec())) + print("Time spent engaged: " + str((time_end_engagement - time_start_engagement).to_sec()) + " seconds") + + # if vehicle_number == 1: + # speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_1_rosbag) + # veh_1_speed_limit_zone_change_times = speed_limit_zone_change_times + # #generate_speed_plot(vehicle_1_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) + # #generate_crosstrack_plot(vehicle_1_rosbag, time_start_engagement, time_end_engagement, bag_file) + # if vehicle_number == 2: + # speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_2_rosbag) + # #generate_speed_plot(vehicle_2_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) + # #generate_crosstrack_plot(vehicle_2_rosbag, time_start_engagement, time_end_engagement, bag_file) + # if vehicle_number == 3: + # speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_3_rosbag) + # #generate_speed_plot(vehicle_3_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) + # #generate_crosstrack_plot(vehicle_3_rosbag, time_start_engagement, time_end_engagement, bag_file) + + if test_type == "VOICES-SIT1": + # Vehicle 2 joins Vehicle 1 from rear; then Vehicle 3 joins 1/2 from Rear + + # Platoon Formation - Joiner Perspective (IHP2 metrics 1,2,3,7,8,9) + vehicle_2_platooning_times = joiner_platoon_formation_analysis(vehicle_2_rosbag, test_type, vehicle_1_type, vehicle_2_type, 2) # [time_received_initial_ack, time_received_second_ack] + vehicle_3_platooning_times = joiner_platoon_formation_analysis(vehicle_3_rosbag, test_type, vehicle_1_type, vehicle_3_type, 3) # [time_received_initial_ack, time_received_second_ack] + vehicle_1_platooning_times = leader_platoon_formation_analysis(vehicle_1_rosbag, test_type, vehicle_1_type, vehicle_2_type, vehicle_3_type, time_1_end_engagement) # [time_began_platooning_with_veh2, time_began_platooning_with_veh3, time_leader_end_engagement] + + vehicle_1_platoon_duration = (vehicle_1_platooning_times[2] - vehicle_1_platooning_times[0]) + vehicle_2_platoon_duration = (vehicle_1_platooning_times[2] - vehicle_1_platooning_times[0]) + vehicle_3_platoon_duration = (vehicle_1_platooning_times[2] - vehicle_1_platooning_times[1]) + vehicle_2_platooning_times.append(vehicle_2_platooning_times[1] + vehicle_2_platoon_duration) # Now [time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] + vehicle_3_platooning_times.append(vehicle_3_platooning_times[1] + vehicle_3_platoon_duration) # Now [time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] + + + print("Vehicle 1 platoon duration: " + str(vehicle_1_platoon_duration.to_sec())) + print("Vehicle 2 platoon duration: " + str(vehicle_2_platoon_duration.to_sec())) + print("Vehicle 3 platoon duration: " + str(vehicle_3_platoon_duration.to_sec())) + + print("DEBUG: Vehicle 1 Platoon Start Time: " + str(vehicle_1_platooning_times[1].to_sec())) + print("DEBUG: Vehicle 1 Platoon End Time: " + str(vehicle_1_platooning_times[2].to_sec())) + + print("DEBUG: Vehicle 2 Platoon Start Time: " + str(vehicle_2_platooning_times[1].to_sec())) + print("DEBUG: Vehicle 2 Platoon End Time: " + str(vehicle_2_platooning_times[2].to_sec())) + + print("DEBUG: Vehicle 3 Platoon Start Time: " + str(vehicle_3_platooning_times[1].to_sec())) + print("DEBUG: Vehicle 3 Platoon End Time: " + str(vehicle_3_platooning_times[2].to_sec())) + + vehicle_1_engagement_duration = time_1_end_engagement - time_1_start_engagement + vehicle_2_platooning_times.insert(0, vehicle_2_platooning_times[2] - vehicle_1_engagement_duration) # Now [time_veh1_engaged, time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] + vehicle_3_platooning_times.insert(0, vehicle_3_platooning_times[2] - vehicle_1_engagement_duration) # Now [time_veh1_engaged, time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] + + print("Vehicle 1: " + str(vehicle_1_platooning_times)) + print("Vehicle 2: " + str(vehicle_2_platooning_times)) + print("Vehicle 3: " + str(vehicle_3_platooning_times)) + + #received_tcm_analysis(vehicle_1_rosbag, time_vehicle_1_platooning_start, test_type, 1, expected_tcm_count=7) + #received_tcm_analysis(vehicle_2_rosbag, vehicle_2_platooning_times[1], test_type, 2, expected_tcm_count=7) + #received_tcm_analysis(vehicle_3_rosbag, vehicle_3_platooning_times[1], test_type, 3, expected_tcm_count=7) + + print("IHP2-6 N/A for SLR Test; no platoon-specific TCMs") + print("IHP2-10 N/A for SLR Test; TCM speed limit matches original speed limit") + print("IHP2-11 N/A for SLR Test; no headway-specific TCMs") + print("IHP2-12 N/A for SLR Test; TCM speed limit matches original speed limit") + print("IHP2-13 N/A for SLR Test; no headway-specific TCMs") + + #downtrack_distance_analysis(vehicle_2_rosbag, vehicle_1_rosbag, test_type, 2, 1, vehicle_2_platooning_times[0]) + #downtrack_distance_analysis(vehicle_3_rosbag, vehicle_2_rosbag, test_type, 3, 2, vehicle_3_platooning_times[0]) + + generate_platooning_plot(vehicle_2_rosbag, vehicle_2_platooning_times[2], vehicle_2_platooning_times[3], vehicle_2_rosbag_name, test_type, 2) + generate_platooning_plot(vehicle_3_rosbag, vehicle_3_platooning_times[2], vehicle_3_platooning_times[3], vehicle_3_rosbag_name, test_type, 3) + + # Get vehicle 2 time when vehicle 1 engages + + # Get vehicle 3 time when vehicle 1 engages + + generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_1_start_engagement, vehicle_1_platooning_times, vehicle_2_platooning_times, vehicle_3_platooning_times, test_type, test_num=2) + + print("Analysis complete") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_wz_rosbags.py b/engineering_tools/data_processing/analyze_wz_rosbags.py new file mode 100644 index 0000000000..ca03063b63 --- /dev/null +++ b/engineering_tools/data_processing/analyze_wz_rosbags.py @@ -0,0 +1,1299 @@ +#!/usr/bin/python3 + +# Copyright (C) 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. + +from inspect import TPFLAGS_IS_ABSTRACT +import sys +import csv +import matplotlib.pyplot as plt +import rospy +import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" +import datetime +import math + +# HOW TO USE SCRIPT: +# Run the following in a terminal to download dependencies: +# sudo add-apt-repository ppa:deadsnakes/ppa +# sudo apt-get update +# sudo apt install python3.7 +# python3.7 -m pip install --upgrade pip +# python3.7 -m pip install matplotlib +# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg +# python3.7 -m pip install lz4 +# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ +# In terminal, navigate to the directory that contains this python script and run the following: +# python3.7 analyze_wz_rosbags.py + +def generate_speed_plot(bag): + # Get the vehicle speed and plot it + speed_received_first_msg = 0.0 + first = True + times = [] + speeds = [] + crosstracks = [] + downtracks = [] + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status']): + #for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): + if first: + time_start = t + first = False + continue + + times.append((t-time_start).to_sec()) + speeds.append(msg.speed * 0.621371) # Conversion from kph to mph + #crosstracks.append(msg.cross_track) + #downtracks.append(msg.down_track) + + plt.plot(times,speeds) + #plt.plot(times,crosstracks) + #plt.plot(times,downtracks) + plt.show() + + return + + +# Helper Function: Get times associated with the system entering the geofence and exiting the geofence +def get_geofence_entrance_and_exit_times(bag): + # Initialize geofence entrance and exit times + time_enter_active_geofence = rospy.Time() # Time that the vehicle has entered the geofence + time_exit_active_geofence = rospy.Time() # Time that the vehicle has exited the geofence-------------------------------------------------------------------------------------------------------- + + # Find geofence entrance and exit times + is_on_active_geofence = False + found_geofence_entrance_time = False + found_geofence_exit_time = False + for topic, msg, t in bag.read_messages(topics=['/environment/active_geofence']): + # If first occurrence of being in the active geofence, set the start time + if (msg.is_on_active_geofence and not is_on_active_geofence): + time_enter_active_geofence = t + #print("Entered geofence at " + str(t.to_sec())) + #print(msg) + found_geofence_entrance_time = True + is_on_active_geofence = True + + # If final occurrence of being in the active geofence, set the end time + if (not msg.is_on_active_geofence and is_on_active_geofence): + time_exit_active_geofence = t + found_geofence_exit_time = True + + time_in_geofence = (time_exit_active_geofence - time_enter_active_geofence).to_sec() + print("Spent " + str(time_in_geofence) + " sec in geofence. Started at " + str(time_enter_active_geofence.to_sec())) + is_on_active_geofence = False + #break + + # Check if both geofence start and end time were found + found_geofence_times = False + if (found_geofence_entrance_time and found_geofence_exit_time): + found_geofence_times = True + + return time_enter_active_geofence, time_exit_active_geofence, found_geofence_times + +# Helper Function: Get the original speed limit for the lanelets within the vehicle's route +# Note: Assumes that all lanelets in the route share the same speed limit prior to the first geofence CARMA Cloud message being processed. +def get_route_original_speed_limit(bag, time_test_start_engagement): + # Initialize the return variable + original_speed_limit = 0.0 + + # Find the speed limit associated with the first lanelet when the system first becomes engaged + for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_test_start_engagement): + original_speed_limit = msg.speed_limit + break + + return original_speed_limit + +# Helper function: Begin with the time that the vehicle exits the active geofence according to +# /guidance/active_geofence topic. This helper function adjusts the time to be +# based on /guidance/route_state in order to be more accurate. +def adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit): + # Get the true time of the end of the geofence, based on when /guidance/route_state displays the + # original speed limit for the current vehicle location + for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence): + if msg.speed_limit == original_speed_limit: + true_time_exit_geofence = t + break + + return true_time_exit_geofence + +# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section +def get_test_case_engagement_times(bag, time_enter_active_geofence, time_exit_active_geofence): + # Initialize system engagement start and end times + time_start_engagement = rospy.Time() + time_stop_engagement = rospy.Time() + + # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section + is_engaged = False + found_engagement_times = False + has_reached_geofence_entrance = False + has_reached_geofence_exit = False + for topic, msg, t in bag.read_messages(topics=['/guidance/state']): + # If entering engagement, track this start time + if (msg.state == 4 and not is_engaged): + time_start_engagement = t + is_engaged = True + + # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance + # state can be published. + if (msg.state == 4): + time_last_engaged = t + + # If exiting engagement, check whether this period of engagement included the geofence entrance and exit times + elif (msg.state != 4 and is_engaged): + is_engaged = False + time_stop_engagement = t + + # Check if the engagement start time was before the geofence entrance and exit times + if (time_start_engagement <= time_enter_active_geofence and t >= time_enter_active_geofence): + has_reached_geofence_entrance = True + if (time_start_engagement <= time_exit_active_geofence and t >= time_exit_active_geofence): + has_reached_geofence_exit = True + + # Set flag if this engagement period includes both the geofence entrance and exit times + if (has_reached_geofence_entrance and has_reached_geofence_exit): + found_test_case_engagement_times = True + break + + # If CARMA ended engagement before guidance state could be updated, check if the last recorded + # time of engagement came after exiting the geofence + if not found_engagement_times: + if time_last_engaged >= time_exit_active_geofence: + time_stop_engagement = time_last_engaged + found_engagement_times = True + + return time_start_engagement, time_stop_engagement, found_engagement_times + +# Helper Function: Get the lanelet IDs that are included in the geofence. +def get_geofence_lanelets(bag, time_start_engagement, advisory_speed_limit): + # Loop through route_state topic and add all unique + lanelets_in_geofence = [] + for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): + if (msg.speed_limit - advisory_speed_limit < 0.01): + if msg.lanelet_id not in lanelets_in_geofence: + lanelets_in_geofence.append(msg.lanelet_id) + + print("Lanelet IDs in the geofence: " + str(lanelets_in_geofence)) + + return lanelets_in_geofence + +# Helper Function: Get the route downtrack associated with the geofence entrance +def get_geofence_entrance_downtrack(bag, time_enter_geofence): + + # Return the first 'downtrack' published to /guidance/route_state after entering the geofence + downtrack_enter_geofence = 0.0 + for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_enter_geofence): + downtrack_enter_geofence = msg.down_track + break + + print("Downtrack of geofence entrance: " + str(downtrack_enter_geofence) + " meters") + + return downtrack_enter_geofence + +# Helper Function: Print out the times associated with the vehicle entering each new lanelet according to /guidance/route_state +def print_lanelet_entrance_times(bag, time_start_engagement): + # Print out time vehicle enters each lanelet according to /guidance/route_state + id = 0 + print("/guidance/route_state lanelet change times:") + for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): + if msg.lanelet_id != id: + print("Time: " + str(t.to_sec()) + "; Lanelet: " + str(msg.lanelet_id) + "; Speed Limit: " + str(msg.speed_limit)) + id = msg.lanelet_id + + return + +########################################################################################################### +# Workzone WZ-1: The geofenced area is a part of the initial route plan. +# +# Workzone WZ-5: The vehicle receives a message from CC that includes the closed lane ahead. The vehicle +# processes this closed lane information. +########################################################################################################### +def check_geofence_in_initial_route(bag, closed_lanelets): + # Get each set route from the bag file (includes set routes and updated/re-rerouted routes) + shortest_path_lanelets = [] + for topic, msg, t in bag.read_messages(topics=['/guidance/route']): + # Print as Debug Statement + print("Shortest Path Route Update at " + str(t.to_sec()) + ": " + str(msg.shortest_path_lanelet_ids)) + + shortest_path_lanelets.append([]) + for lanelet_id in msg.shortest_path_lanelet_ids: + shortest_path_lanelets[-1].append(lanelet_id) + + # If there are two route paths, check that the first (original) route contains the closed lanelet(s) and the second route doesn't + # Note: Assumes there should be only two routes: (1) the initial route and (2) the re-routed route + initial_route_includes_closed_lane = False # Flag for B-1 success + map_is_updated_for_closed_lane = False # Flag for B-11 success + if (len(shortest_path_lanelets) == 2): + original_shortest_path = shortest_path_lanelets[0] + rerouted_shortest_path = shortest_path_lanelets[1] + + for lanelet_id in closed_lanelets: + if lanelet_id in original_shortest_path: + initial_route_includes_closed_lane = True + else: + initial_route_includes_closed_lane = False + break + + for lanelet_id in closed_lanelets: + if lanelet_id not in rerouted_shortest_path: + map_is_updated_for_closed_lane = True + else: + map_is_updated_for_closed_lane = False + break + else: + print("Invalid quantity of route updates found in bag file (" + str(len(shortest_path_lanelets)) + " found, 2 expected)") + + # Print result statements and return success flags + if (initial_route_includes_closed_lane): + print("WZ-1 succeeded; all closed lanelets " + str(closed_lanelets) + " were in the initial route") + else: + print("WZ-1 failed: not all closed lanelets " + str(closed_lanelets) + " were in the initial route.") + + if (map_is_updated_for_closed_lane): + print("WZ-5 succeeded: no closed lanelets " + str(closed_lanelets) + " were in the re-routed route.") + else: + print("WZ-5 failed: at least 1 closed lanelet " + str(closed_lanelets) + " was in the re-routed route.") + + return initial_route_includes_closed_lane, map_is_updated_for_closed_lane + +########################################################################################################### +# Workzone WZ-2: Amount of time that the vehicle is going at steady state (e.g. same lane, constant speed) +# before it receives the first CC message. (> 3 seconds) +########################################################################################################### +def check_steady_state_before_first_received_message(bag, time_start_engagement, time_received_first_message, original_speed_limit): + # (m/s) Threshold offset of vehicle speed to speed limit to be considered at steady state + threshold_speed_limit_offset = 0.89408 # 0.89408 m/s is 1 mph + min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset + max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset + + # (seconds) Minimum time between vehicle reaching steady state and first TIM MobilityOperation message being received + min_time_between_steady_state_and_msg = 10.0 + + # Get the time that the vehicle reaches within the set offset of the speed limit (while system is engaged) + time_start_steady_state = 0.0 + has_reached_steady_state = False + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement): + current_speed = msg.twist.linear.x # Current vehicle speed in m/s + if (max_steady_state_speed >= current_speed >= min_steady_state_speed): + has_reached_steady_state = True + time_start_steady_state = t + break + + # Check if the time the vehicle reaches steady state is more than 'min_time_between_steady_state_and_msg' seconds before the first received TIM message + if (has_reached_steady_state): + time_between_steady_state_and_msg = (time_received_first_message - time_start_steady_state).to_sec() + if (time_between_steady_state_and_msg >= min_time_between_steady_state_and_msg): + is_successful = True + print("WZ-2 succeeded; reached steady state " + str(time_between_steady_state_and_msg) + " seconds before receiving first TIM or TCM message.") + else: + is_successful = False + if (time_between_steady_state_and_msg > 0): + print("WZ-2 failed; reached steady state " + str(time_between_steady_state_and_msg) + " seconds before receiving first TIM or TCM message.") + else: + print("WZ-2 failed; reached steady state " + str(-time_between_steady_state_and_msg) + " seconds after receiving first TIM or TCM message.") + else: + print("WZ-2 failed; vehicle never reached steady state during rosbag recording.") + is_successful = False + + return is_successful + +########################################################################################################### +# Workzone WZ-4: The vehicle receives a message from CC that includes the new speed limit ahead. The vehicle +# processes this new speed limit. +# +# Workzone WZ-5: The vehicle receives a message from CC that includes the closed lane ahead. The vehicle +# processes this closed lane information. +# +# Workzone WZ-6: The vehicle receives a message from CC that includes the "taper right" closed lane ahead. +# The vehicle processes this closed lane information. +# +# Workzone WZ-7: The vehicle receives a message from CC that includes the "open right" closed lane ahead. +# The vehicle processes this closed lane information. +# +# Workzone WZ-8: The vehicle receives a message from CC that includes the "open right" closed lane ahead. +# The vehicle processes this closed lane information. +########################################################################################################### +def get_workzone_TCM_data(bag): + # Check that TCM Messages are received for closed lane, open right, taper right, direction reversal, and advisory speed + has_closed_lane = False + has_advisory_speed = False + has_direction_reversal = False + has_taper_right = False + has_open_right = False + advisory_speed = 0.0 + time_first_msg_received = rospy.Time() + first = True + for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): + if first: + time_first_msg_received = t + first = False + + if msg.tcmV01.params.detail.choice == 5 and msg.tcmV01.params.detail.closed == 1: + #print(msg) + has_closed_lane = True + elif msg.tcmV01.params.detail.choice == 5 and msg.tcmV01.params.detail.closed == 3: + latitude = msg.tcmV01.geometry.reflat + longitude = msg.tcmV01.geometry.reflon + print("Taper right has stop bar location at lat=" + str(latitude) + ", longitude=" + str(longitude)) + has_taper_right = True + elif msg.tcmV01.params.detail.choice == 5 and msg.tcmV01.params.detail.closed == 5: + #print(msg) + has_open_right = True + elif msg.tcmV01.params.detail.choice == 7 and msg.tcmV01.params.detail.direction == 1: + #print(msg) + has_direction_reversal = True + elif msg.tcmV01.params.detail.choice == 12: + #print(msg) + has_advisory_speed = True + advisory_speed = msg.tcmV01.params.detail.maxspeed + + if has_closed_lane and has_advisory_speed and has_direction_reversal and has_taper_right and has_open_right: + print("TCM Messages Received: Closed Lane; Direction Reversal; Taper Right; Open Right; Advisory Speed: " + str(advisory_speed) + \ + " mph)") + break + + wz_4_successful = False + if has_advisory_speed: + print("WZ-4 succeeded; TCM message received with advisory speed limit " + str(advisory_speed) + " mph") + wz_4_successful = True + else: + print("WZ-4 failed; no TCM message with an advisory speed limit was received.") + + wz_5_successful = False + if has_closed_lane: + print("WZ-5 succeeded; TCM message received with closed lane") + wz_5_successful = True + else: + print("WZ-5 failed; no TCM message with closed lane.") + + wz_6_successful = False + if has_taper_right: + print("WZ-6 succeeded; TCM message received with a taper right closed lane") + wz_6_successful = True + else: + print("WZ-6 failed; no TCM message with a taper right closed lane was received.") + + wz_7_successful = False + if has_open_right: + print("WZ-7 succeeded; TCM message received with an open right closed lane") + wz_7_successful = True + else: + print("WZ-7 failed; no TCM message with an open right closed lane was received.") + + wz_8_successful = False + if has_direction_reversal: + print("WZ-8 succeeded; TCM message received with a direction reversal") + wz_8_successful = True + else: + print("WZ-8 failed; no TCM message with a direction reversal was received.") + + return advisory_speed, time_first_msg_received, wz_4_successful, wz_5_successful, wz_6_successful, wz_7_successful, wz_8_successful + +########################################################################################################### +# Workzone WZ-9: The vehicle shall continuously receive SPAT messages with a time gap between sequentially +# received SPAT messages below 0.5 seconds at least 90% of the time while engaged. +########################################################################################################### +def check_percentage_successful_spat_msg(bag, time_start_engagement, time_end_engagement): + # Parameters used for the computation of this metric + min_percent_time_successful = 90.0 # (90%); Percent of active platooning time that the maximum or better 'SPAT' frequency must be achieved + max_duration_between_msgs = 0.50 # (Seconds); Maximum duration between sequentially broadcasted 'SPAT' messages + + # Obtain the quantity of SPAT messages received during engagement + duration_since_prev_msg = 0.0 + total_time_between_unsuccessful_msgs = 0.0 + total_time_between_successful_msgs = 0.0 + largest_duration_between_msgs = 0.0 + total_time = 0.0 + total_time_between_unsuccessful_msgs = 0.0 + count_msgs = 0 + count_msgs_above_max_duration = 0 + time_prev_msg = rospy.Time() + first = True + current_signal_state = 0 + for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_start_engagement, end_time = time_end_engagement): + if first: + time_prev_msg = t + first = False + continue + + for movement in msg.intersection_state_list: + signal_state = movement.movement_list[1].movement_event_list[0].event_state.movement_phase_state + if (signal_state != current_signal_state): + print("Transitioned from signal " + str(current_signal_state) + " to " + str(signal_state) + " at " + str(t.to_sec())) + current_signal_state = signal_state + #print("***************************") + #print(len(movement.movement_list)) + #print(str(count_msgs) + " msg has phase:") + #print(str(movement.movement_list[0].movement_event_list[0].event_state.movement_phase_state) + " for SG_ID " + str(movement.movement_list[0].signal_group)) + #print("SG_ID " + str(movement.movement_list[1].signal_group) + " has signal " + str(movement.movement_list[1].movement_event_list[0].event_state.movement_phase_state)) + + #for move in movement.movement_list: + # for event in move.movement_event_list: + # print(event.event_state.movement_phase_state) + # msg.intersection_state_list.movement_list.event_state.movement_phase_state + + + + duration_since_prev_msg = (t - time_prev_msg).to_sec() + time_prev_msg = t + #print("Duration between SPAT messages: " + str(duration_since_prev_msg) + " sec") + + # Track the number of messages received after the (1/min_frequency) duration just for debugging purposes + if (duration_since_prev_msg >= max_duration_between_msgs): + total_time_between_unsuccessful_msgs += duration_since_prev_msg + count_msgs_above_max_duration += 1 + else: + total_time_between_successful_msgs += duration_since_prev_msg + + if duration_since_prev_msg > largest_duration_between_msgs: + largest_duration_between_msgs = duration_since_prev_msg + + total_time += duration_since_prev_msg + count_msgs += 1 + + percent_time_successful = (total_time_between_successful_msgs / total_time) * 100.0 + + # Update is_successful flag and print debug statements + is_successful = False + if percent_time_successful >= min_percent_time_successful: + print("WZ-9 Succeeded; Time between SPAT messages was below " + str(round(max_duration_between_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ + "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") + is_successful = True + else: + print("WZ-9 Failed; Time between SPAT messages was below " + str(round(max_duration_between_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ + "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") + is_successful = False + + pct_above_max_duration = (float(count_msgs_above_max_duration) / float(count_msgs)) * 100.0 + print(str(count_msgs) + " Total SPAT Messages sent (" + str(round(100-pct_above_max_duration,3)) + "% Successful); " + str(count_msgs_above_max_duration) + " after " + str(round(max_duration_between_msgs,3)) + " sec (" \ + + str(round(pct_above_max_duration,3)) + "%);; Longest duration " \ + + " between SPAT messages was " + str(largest_duration_between_msgs) + " sec") + + return is_successful + +########################################################################################################### +# Workzone WZ-10: The vehicle shall never receive sequential SPAT messages with a time gap between messages +# that exceeds 1 second. +########################################################################################################### +def check_duration_between_spat_msg_below_max_duration(bag, time_start_engagement, time_end_engagement): + # Parameters used for the computation of this metric + max_duration_between_msgs = 1.00 # (Seconds); Max duration between sequentially broadcasted 'SPAT' messages + + # Obtain the quantity of SPAT messages received during engagement + duration_since_prev_msg = 0.0 + total_time_between_unsuccessful_msgs = 0.0 + total_time_between_successful_msgs = 0.0 + largest_duration_between_msgs = 0.0 + total_time = 0.0 + total_time_between_unsuccessful_msgs = 0.0 + count_msgs = 0 + count_msgs_above_max_duration = 0 + time_prev_msg = rospy.Time() + first = True + for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_start_engagement, end_time = time_end_engagement): + if first: + time_prev_msg = t + first = False + continue + + m = msg + duration_since_prev_msg = (t - time_prev_msg).to_sec() + time_prev_msg = t + #print("Duration between SPAT messages: " + str(duration_since_prev_msg) + " sec") + + # Track the number of messages received after the (1/min_frequency) duration just for debugging purposes + if (duration_since_prev_msg >= max_duration_between_msgs): + total_time_between_unsuccessful_msgs += duration_since_prev_msg + count_msgs_above_max_duration += 1 + print(str(duration_since_prev_msg) + " sec between SPAT messages") + else: + total_time_between_successful_msgs += duration_since_prev_msg + + if duration_since_prev_msg > largest_duration_between_msgs: + largest_duration_between_msgs = duration_since_prev_msg + + total_time += duration_since_prev_msg + count_msgs += 1 + #print(msg) + percent_time_successful = (total_time_between_successful_msgs / total_time) * 100.0 + + # Update is_successful flag and print debug statements + is_successful = False + if count_msgs_above_max_duration == 0: + print("WZ-10 Succeeded; " + str(count_msgs_above_max_duration) + " SPAT messages more than 1.0 sec apart") + is_successful = True + else: + print("WZ-10 Succeeded; " + str(count_msgs_above_max_duration) + " SPAT messages more than 1.0 sec apart") + is_successful = False + + return is_successful + +########################################################################################################### +# Workzone WZ-11: After the vehicle processes a received SPAT message and determines that it will arrive at +# a red or yellow signal indication, the vehicle's actual trajectory in preparation for a +# stop will include a deceleration section. The average deceleration over the entire section +# shall be no less than 1 m/s^2, and the average deceleration over any 1-second portion of +# the section shall be no greater than 2.0 m/s^2. +########################################################################################################### +def check_deceleration_for_red_light(bag, time_start_engagement): + # Obtain the timestamp at which the vehicle comes to a complete stop at the red light + time_stopped = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement): + if (t-time_start_engagement).to_sec() < 5.0: + continue + + if msg.speed < 0.10: + time_stopped = t + print("Vehicle came to complete stop " + str((time_stopped - time_start_engagement).to_sec()) + " seconds after engagement") + break + + # Verify that light indication is RED (signal phase 3) at time_stopped + one_second_duration = rospy.Duration(1.0) + for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_stopped - one_second_duration): + signal_at_stop = msg.intersection_state_list[0].movement_list[1].movement_event_list[0].event_state.movement_phase_state + + if (signal_at_stop == 3): + print("Vehicle came to a stop at a Red Signal Indication: " + str(signal_at_stop)) + elif (signal_at_stop == 6): + print("Vehicle came to a stop at a Green Signal Indication: " + str(signal_at_stop)) + elif (signal_at_stop == 8): + print("Vehicle came to a stop at a Yellow Signal Indication: " + str(signal_at_stop)) + + break + + # Obtain time_last_accel, which is the last acceleration before decelerating to a complete stop + prev_speed = 0.0 + first = True + time_last_accel = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement, end_time = time_stopped-one_second_duration): + if first: + prev_speed = msg.speed + first = False + continue + + delta_speed = msg.speed - prev_speed + if delta_speed >= 0: + time_last_accel = t + speed_after_final_increase = msg.speed * 0.277777 # Conversion from kph to m/s + + prev_speed = msg.speed + + print("Final speed increase before stop occurred at " + str(time_last_accel.to_sec()) + ", " + str((time_last_accel-time_start_engagement).to_sec()) + " seconds after engagement") + print("Speed after final acceleration before decelerating to a stop was " + str(speed_after_final_increase) + " m/s ") + + # Obtain average decelation over all 1-second windows in deceleration section + all_one_second_windows_successful = True + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_last_accel, end_time = (time_stopped-one_second_duration)): + speed_initial = msg.speed * 0.277777 # Conversion from kph to m/s + time_initial = t + + speed_final = 0.0 + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): + speed_final = msg.speed * 0.277777 # Conversion from kph to m/s + t_final = t + break + + one_second_decel = (speed_initial - speed_final) / (t_final-time_initial).to_sec() + + if one_second_decel > 2.0: + print("Failure: 1-second window had decel rate was " + str(one_second_decel) + " m/s^2; end time was " + str((time_stopped - t_final).to_sec()) + " seconds before stopping") + all_one_second_windows_successful = False + #else: + # print("Success: 1-second window has decel rate at " + str(one_second_decel) + " m/s^2") + + # Obtain the average deceleration over the entire deceleration section + average_total_decel = speed_after_final_increase / (time_stopped-time_last_accel).to_sec() + + total_deceleration_rate_successful = False + if average_total_decel >= 1.0: + print("WZ-11 (total deceleration before red light stop) succeeded; total average deceleration was " + str(average_total_decel) + " m/s^2") + total_deceleration_rate_successful = True + else: + print("WZ-11 (total deceleration before red light stop) failed; total average deceleration was " + str(average_total_decel) + " m/s^2") + + if all_one_second_windows_successful: + print("WZ-11 (1-second window deceleration before red light stop) succeeded; no occurrences of 1-second average deceleration above 2.0 m/s^2") + else: + print("WZ-11 (1-second window deceleration before red light stop) failed; at least one occurrence of 1-second average deceleration above 2.0 m/s^2") + + is_successful = False + if total_deceleration_rate_successful and all_one_second_windows_successful: + is_successful = True + + return is_successful, time_last_accel, time_stopped + +def euclidean_distance(p1, p2): + distance = math.sqrt((p1[0]-p2[0])**2 + (p1[1] - p2[1])**2) + return distance + +########################################################################################################### +# Workzone WZ-12: After the vehicle processes a received SPAT message and determines that it will arrive at +# a red or yellow signal indication, the vehicle will come to a complete stop 0-15 feet before +# the corresponding stop bar for the specified traffic signal. +########################################################################################################### +def check_stop_location_for_red_light(bag, stop_bar_location, dist_rear_axle_to_front_bumper, time_start_engagement, time_start_decel, time_stopped): + # Obtain true stopping location in map frame + system_stop_location = [0,0] + for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_stopped): + system_stop_location[0] = msg.pose.position.x + system_stop_location[1] = msg.pose.position.y + print("Red Light: System stopped at map location x=" + str(system_stop_location[0]) + ", y=" + str(system_stop_location[1])) + break + + # Obtain the location of the system at the start of engagement + system_start_location = [0,0] + for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_start_engagement): + system_start_location[0] = msg.pose.position.x + system_start_location[1] = msg.pose.position.y + break + + dist_start_to_vehicle_stop = euclidean_distance(system_start_location, system_stop_location) + dist_start_to_stop_bar = euclidean_distance(system_start_location, stop_bar_location) + dist_vehicle_to_stop_bar = euclidean_distance(stop_bar_location, system_stop_location) + print("Distance start to stop: " + str(dist_start_to_vehicle_stop) + " meters") + print("Distance start to stop bar: " + str(dist_start_to_stop_bar) + " meters") + print("Distance stop bar to stop: " + str(dist_vehicle_to_stop_bar) + " meters") + + is_successful = False + if dist_start_to_vehicle_stop < dist_start_to_stop_bar: + dist_vehicle_to_stop_bar = dist_vehicle_to_stop_bar - dist_rear_axle_to_front_bumper + if dist_vehicle_to_stop_bar < 0: + print("WZ-12 Failed; Vehicle stopped with front bumper " + str(abs(dist_vehicle_to_stop_bar)) + " meters after stop bar") + elif dist_vehicle_to_stop_bar < 15.0: + print("WZ-12 Succeeded; Vehicle stopped with front bumper " + str(dist_vehicle_to_stop_bar) + " meters in front of stop bar") + is_successful = True + else: + print("WZ-12 Failed; Vehicle stopped with front bumper " + str(dist_vehicle_to_stop_bar) + " meters before stop bar") + else: + dist_vehicle_to_stop_bar = dist_vehicle_to_stop_bar + dist_rear_axle_to_front_bumper + print("WZ-12 Failed; Vehicle stopped with front bumper " + str(dist_vehicle_to_stop_bar) + " meters after stop bar") + + return is_successful + +########################################################################################################### +# Workzone WZ-13: If the vehicle is stopped for a red or yellow signal indication and receives a new SPAT +# message for a green signal indication, the vehicle will begin accelerating within 0-4 seconds. +########################################################################################################### +def check_acceleration_time_after_green_light(bag, time_stopped): + max_time_to_start_accel = 4.0 # (Seconds) + + # Get occurrence of time_start_green_light after coming to a stop + time_start_green_light = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_stopped): + signal_at_stop = msg.intersection_state_list[0].movement_list[1].movement_event_list[0].event_state.movement_phase_state + + if signal_at_stop == 6: + time_start_green_light = t + break + + # Get timestamp of first acceleration after time_start_green_lightm + time_start_accel = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_green_light): + if msg.speed > 0.1: + time_start_accel = t + break + + time_to_start_accel = (time_start_accel-time_start_green_light).to_sec() + + is_successful = False + if time_to_start_accel <= max_time_to_start_accel: + print("WZ-13 Succeeded; after green light, system took " + str(time_to_start_accel) + " seconds to begin accelerating (less than 4)") + is_successful = True + else: + print("WZ-13 Succeeded; after green light, system took " + str(time_to_start_accel) + " seconds to begin accelerating (more than 4)") + + return is_successful + +########################################################################################################### +# Workzone WZ-14: If the vehicle is stopped for a red or yellow signal indication and receives a new SPAT +# message for a green signal indication, the actual trajectory back to normal operations +# will include an acceleration section. The average acceleration over the entire section +# shall be no less than 1 m/s^2, and the average acceleration over any 1-second portion of +# the section shall be no greater than 2.0 m/s^2. +########################################################################################################### +def check_acceleration_after_stop(bag, time_stopped): + # Obtain time_start_accel + time_start_accel = rospy.Time() + speed_start_accel = 0.0 + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_stopped): + if msg.speed >= 0.1: + speed_start_accel = msg.speed * 0.277778 # Conversion kph to m/s + time_start_accel = t + break + + # Obtain time_end_accel (first deceleration after starting accel; also print the speed at end of accel) + prev_speed = 0.0 + first = True + time_end_accel = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_accel): + if first: + prev_speed = msg.speed + first = False + continue + + delta_speed = msg.speed - prev_speed + if delta_speed <= 0: + time_end_accel = t + speed_end_accel = msg.speed * 0.277778 # Conversion kph to m/s + speed_after_final_increase_mph = msg.speed * 0.621371 # Conversion from kph to mph + print("Speed at end of accel after light turns green: " + str(speed_after_final_increase_mph) + " mph") + break + + prev_speed = msg.speed + + # Check the total average + total_average_decel = (speed_end_accel - speed_start_accel) / (time_end_accel-time_start_accel).to_sec() + print("Total average decel: " + str(total_average_decel) + " m/s^2") + + # Check the 1-second window averages + # Obtain average decelation over all 1-second windows in acceleration section + one_second_duration = rospy.Duration(1.0) + all_one_second_windows_successful = True + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_accel, end_time = (time_end_accel-one_second_duration)): + speed_initial = msg.speed * 0.277777 # Conversion from kph to m/s + time_initial = t + + speed_final = 0.0 + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): + speed_final = msg.speed * 0.277777 # Conversion from kph to m/s + t_final = t + break + + one_second_decel = (speed_final - speed_initial) / (t_final-time_initial).to_sec() + + if one_second_decel > 2.0: + print("Failure: 1-second window had accel rate was " + str(one_second_decel) + " m/s^2; end time was " + str((time_stopped - t_final).to_sec()) + " seconds before stopping") + all_one_second_windows_successful = False + #else: + # print("Success: 1-second window has accel rate at " + str(one_second_decel) + " m/s^2") + + # Print success/failure statements + total_deceleration_rate_successful = False + if total_average_decel >= 1.0: + print("WZ-14 (total accel after complete stop) succeeded; total average acceleration was " + str(total_average_decel) + " m/s^2") + total_deceleration_rate_successful = True + else: + print("WZ-14 (total accel after complete stop) failed; total average acceleration was " + str(total_average_decel) + " m/s^2") + + if all_one_second_windows_successful: + print("WZ-14 (1-second accel after complete stop) succeeded; no occurrences of 1-second average acceleration above 2.0 m/s^2") + else: + print("WZ-14 (1-second accel after complete stop) failed; at least one occurrence of 1-second average acceleration above 2.0 m/s^2") + + is_successful = False + if total_deceleration_rate_successful and all_one_second_windows_successful: + is_successful = True + + return is_successful + + +########################################################################################################### +# Workzone WZ-15: If the vehicle processes a received SPAT message and determines that it will arrive at a +# green traffic signal indication, the vehicle shall travel past the traffic signal at a speed +# of +/- 2 mph of the speed limit while the signal indication is green. +########################################################################################################### +def check_vehicle_speed_at_green_light(bag, advisory_speed_limit, stop_bar_location, time_start_engagement): + # Get time_vehicle_at_stop_bar (which is the time it passes the stop bar line) + min_dist_to_stop_bar = 1.0 # meteres + vehicle_location = [0,0] + time_vehicle_at_stop_bar = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_start_engagement): + vehicle_location[0] = msg.pose.position.x + vehicle_location[1] = msg.pose.position.y + if (euclidean_distance(vehicle_location, stop_bar_location) <= min_dist_to_stop_bar): + time_vehicle_at_stop_bar = t + break + + # Get traffic signal value at time_pass_green_light + has_passed_at_green_light = False + for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_vehicle_at_stop_bar): + signal_at_stop = msg.intersection_state_list[0].movement_list[1].movement_event_list[0].event_state.movement_phase_state + + if (signal_at_stop == 3): + print("WZ-15 Failed; Vehicle passed light during a Red Signal Indication: ") + elif (signal_at_stop == 6): + print("WZ-15 succeeded; Vehicle passed light during a Green Signal Indication: ") + has_passed_at_green_light = True + elif (signal_at_stop == 8): + print("WZ-15 Failed; Vehicle passed light during a Yellow Signal Indication: ") + + break + + # Get vehicle speed at time_pass_green_light + + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_vehicle_at_stop_bar): + speed_at_light = msg.speed * 0.621371 # Conversion from kph to mph + break + + has_passed_green_light_at_correct_speed = False + original_speed_limit = advisory_speed_limit * 2.23694 # m/s to mph + min_speed = original_speed_limit - 2.0 + max_speed = original_speed_limit + 2.0 + if min_speed <= speed_at_light <= max_speed: + has_passed_at_green_light = True + print("WZ-15 succeeded; vehicle passed light at speed " + str(speed_at_light) + " mph") + else: + print("WZ-15 failed; vehicle passed light at speed " + str(speed_at_light) + " mph") + + + # Print success/failure statements + is_successful = False + if has_passed_at_green_light and has_passed_green_light_at_correct_speed: + is_successful = True + + return is_successful, time_vehicle_at_stop_bar + +########################################################################################################### +# Workzone WZ-16: The actual trajectory in preparation for the geofenced area with an advisory speed limit +# will include a deceleration section. The average deceleration over the entire section shall +# be no less than 1 m/s^2, and the average deceleration over any 1-second portion of the section +# shall be no greater than 2.0 m/s^2. +########################################################################################################### +def check_deceleration_after_green_light(bag, time_vehicle_at_stop_bar, time_start_engagement): + # Obtain the last speed decrease time before reaching stop bar + prev_speed = 0.0 + first = True + time_last_decel = rospy.Time() + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement, end_time = time_vehicle_at_stop_bar): + if first: + prev_speed = msg.speed + first = False + continue + + delta_speed = msg.speed - prev_speed + if delta_speed <= 0: + time_last_decel = t + speed_after_final_decel = msg.speed * 0.277777 # Conversion from kph to m/s + #print("Time " + str(t.to_sec()) + " has speed " + str(speed_after_final_decel)) + + prev_speed = msg.speed + + prev_speed = 0.0 + first = True + time_last_accel = rospy.Time() + one_second_duration = rospy.Duration(1.0) + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement, end_time = time_last_decel - one_second_duration): + if first: + prev_speed = msg.speed + first = False + continue + + delta_speed = msg.speed - prev_speed + if delta_speed >= 0: + time_last_accel = t + speed_after_final_accel = msg.speed * 0.277777 # Conversion from kph to m/s + + prev_speed = msg.speed + + speed_after_final_accel_mph = speed_after_final_accel * 2.23694 + speed_after_final_decel_mph = speed_after_final_decel * 2.23694 + print("(WZ-16): Speed start of decel: " + str(speed_after_final_accel) + " m/s; " + str(speed_after_final_accel_mph) + " mph") + print("(WZ-16): Speed end of decel " + str(speed_after_final_decel) + " m/s; " + str(speed_after_final_decel_mph) + " mph") + speed_start_decel = speed_after_final_accel + speed_end_decel = speed_after_final_decel + + total_average_decel = (speed_start_decel - speed_end_decel) / (time_last_decel - time_last_accel).to_sec() + + + # Obtain average decelation over all 1-second windows in deceleration section + one_second_duration = rospy.Duration(1.0) + all_one_second_windows_successful = True + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_last_accel, end_time = (time_last_decel-one_second_duration)): + speed_initial = msg.speed * 0.277777 # Conversion from kph to m/s + time_initial = t + + speed_final = 0.0 + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): + speed_final = msg.speed * 0.277777 # Conversion from kph to m/s + t_final = t + break + + one_second_decel = (speed_initial - speed_final) / (t_final-time_initial).to_sec() + + if one_second_decel > 2.0: + print("Failure: 1-second window had decel rate was " + str(one_second_decel) + " m/s^2; end time was " + str((time_last_accel - t_final).to_sec()) + " seconds before end of deceleration") + all_one_second_windows_successful = False + #else: + # print("Success: 1-second window has decel rate at " + str(one_second_decel) + " m/s^2") + + # Print success/failure statements + total_deceleration_rate_successful = False + if total_average_decel >= 1.0: + print("WZ-16 (total decel for geofence) succeeded; total average deceleration was " + str(total_average_decel) + " m/s^2") + total_deceleration_rate_successful = True + else: + print("WZ-16 (total decel for geofence) failed; total average deceleration was " + str(total_average_decel) + " m/s^2") + + if all_one_second_windows_successful: + print("WZ-16 (1-second decel for geofence) succeeded; no occurrences of 1-second average deceleration above 2.0 m/s^2") + else: + print("WZ-16 (1-second decel for geofence) failed; at least one occurrence of 1-second average deceleration above 2.0 m/s^2") + + + is_successful = False + if total_deceleration_rate_successful and all_one_second_windows_successful: + is_successful = True + return is_successful + +########################################################################################################### +# Workzone WZ-17: When navigating the "taper right" closed lane section of the route, the vehicle will travel +# sequentially through lanelets 1302928, 1303118, and 1303305. +########################################################################################################### + +########################################################################################################### +# Workzone WZ-18: When navigating the "open right" closed lane section of the route, the vehicle will travel +# sequentially through lanelets 1303305, 1303304, and 1302937. +########################################################################################################### + +########################################################################################################### +# Workzone WZ-19: After exiting the geofenced area with an advisory speed limit, the vehicle will begin +# accelerating back to the original speed limit within 0-30 feet. +########################################################################################################### + +########################################################################################################### +# Workzone WZ-20: After exiting the geofenced area with an advisory speed limit, the actual trajectory back +# to normal operations will include an acceleration section. The average acceleration over +# the entire section shall be no less than 1 m/s^2, and the average acceleration over any +# 1-second portion of the section shall be no greater than 2.0 m/s^2. +########################################################################################################### + +########################################################################################################### +# Workzone WZ-21: When not changing lanes, the vehicle is fully contained within its lane (it does not cross +# either of the lane lines) for at least 95% of the time spent with the CARMA system engaged. +########################################################################################################### + +########################################################################################################### +# Workzone WZ-22: When not changing lanes, the vehicle never crosses the left or right lane line associated +# with its current lane by more than 0.1 meters while the CARMA system is engaged. +########################################################################################################### + +########################################################################################################### +# Workzone WZ-23: After exiting the geofenced area, system is at steady state for at least 3 seconds before +# the end of the use case. +########################################################################################################### +def check_steady_state_after_geofence(bag, time_end_engagement, original_speed_limit): + ten_second_duration = rospy.Duration(10.0) + + + # Parameters used for metric evaluation + # (m/s) Threshold offset from speed limit; vehicle considered at steady state when its speed is within this offset of the speed limit + threshold_speed_limit_offset = 0.894 # 0.894 m/s is 2 mph + # (m/s) Minimum speed to be considered at steady state + min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset + # (m/s) Maximum speed to be considered at steady state + max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset + # (seconds) Minimum required threshold at steady state after completing all geofence-triggered maneuvers + min_steady_state_time = 10.0 + + # Conduct steady state evaluation + # Get the start time of the vehicle reaching steady state (if one exists) + has_steady_state = False + for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_end_engagement - ten_second_duration, end_time = time_end_engagement): + # Vehicle has reached steady state when its speed within threshold range of steady state speed + if (max_steady_state_speed >= msg.twist.linear.x >= min_steady_state_speed): + time_start_steady_state = t + has_steady_state = True + break + + time_steady_state = (time_end_engagement - time_start_steady_state).to_sec() + + if (time_steady_state >= 3.0): + print("WZ-23 succeeded; system ended use case at steady state for " + str(time_steady_state) + " seconds") + is_successful = True + else: + if has_steady_state: + print("WZ-23 failed; system ended use case at steady state for " + str(time_steady_state) + " seconds") + is_successful = False + if not has_steady_state: + print("WZ-23 failed; system did not reach steady state at end of use case.") + is_successful = False + + return is_successful + +########################################################################################################### +# Basic Travel WZ-24: The entire scenario will satisfy all previous criteria using any of the speeds given here for +# the "regular speed limit" (i.e. the speed limit when not in the geo-fence). +########################################################################################################### +def check_speed_limit_when_not_in_geofence(bag, original_speed_limit): + expected_speed_limit = 8.9408 # m/s (20 mph) + + # (m/s) Threshold offset from speed limit to account for floating point precision + threshold_speed_limit_offset = 0.01 + max_speed_limit = expected_speed_limit + threshold_speed_limit_offset # (m/s) + min_speed_limit = expected_speed_limit - threshold_speed_limit_offset # (m/s) + + is_successful = False + if min_speed_limit <= original_speed_limit <= max_speed_limit: + print("WZ-24 Succeeded; speed limit outside of geofence is " + str(original_speed_limit) + " m/s (20 mph)") + is_successful = True + else: + print("WZ-24 Failed; speed limit outside of geofence is " + str(original_speed_limit) + " m/s") + + return is_successful + +########################################################################################################### +# Workzone WZ-25: The advisory speed limit communicated by CARMA Cloud shall be 5 mph less than the +# regular speed limit. +########################################################################################################### +def check_advisory_speed_limit(bag, advisory_speed_limit, original_speed_limit): + # (m/s) Required offset from normal speed limit required for the advisory speed limit + speed_limit_offset = 2.2352 # 4.4704 m/s is 10 mph + # (m/s) Threshold offset from expected advisory speed limit to account for floating point precision + threshold_speed_limit_offset = 0.01 + max_advisory_speed_limit = (original_speed_limit - speed_limit_offset) + threshold_speed_limit_offset + min_advisory_speed_limit = (original_speed_limit - speed_limit_offset) - threshold_speed_limit_offset + + # Evaluate speed limits, print success/failure statement, and return success flag + advisory_speed_limit_mph = int(advisory_speed_limit * 2.23694) # Conversion of m/s to mph + original_speed_limit_mph = int(original_speed_limit * 2.23694) # Conversion of m/s to mph + speed_limit_offset_mph = int(speed_limit_offset * 2.23694) # Conversion of m/s to mph + if (max_advisory_speed_limit >= advisory_speed_limit >= min_advisory_speed_limit): + print("WZ-25 succeeded; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ + + " mph), which is " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ + + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") + is_successful = True + else: + print("WZ-25 failed; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ + + " mph), which is not " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ + + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") + is_successful = False + + return is_successful + +# Main Function; run all tests from here +def main(): + if len(sys.argv) < 2: + print("Need 1 arguments: process_bag.py ") + exit() + + #source_folder = sys.argv[1] + + # Re-direct the output of print() to a specified .txt file: + #orig_stdout = sys.stdout + current_time = datetime.datetime.now() + #text_log_filename = "Results_" + str(current_time) + ".txt" + #text_log_file_writer = open(text_log_filename, 'w') + #sys.stdout = text_log_file_writer + + # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): + #csv_results_filename = "Results_" + str(current_time) + ".csv" + #csv_results_writer = csv.writer(open(csv_results_filename, 'w')) + #csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", + # "WZ-1 Result", "WZ-2 Result", "WZ-3 Result", "WZ-4 Result", "WZ-5 Result", "WZ-6 Result", + # "WZ-7 Result", "WZ-8 Result", "WZ-9 Result", "WZ-10 Result","WZ-11 Result", "WZ-12 Result", + # "WZ-13 Result", "WZ-14 Result", "WZ-15 Result", "WZ-16 Result", "WZ-17 Result", "WZ-18 Result", + # "WZ-19 Result", "WZ-20 Result", "WZ-21 Result", "WZ-22 Result", "WZ-23 Result", "WZ-24 Result", "WZ-25 Result"]) + + # Create list of Red Light Workzone Black Pacifica bag files to be processed + black_pacifica_red_bag_files = [] + + # Create list of Red Light Workzone Ford Fusion bag files to be processed + ford_fusion_red_bag_files = ["_2021-09-22-16-00-28-red.bag", + "_2021-09-22-19-03-26-red.bag"] + + # Create list of Red Light Workzone Blue Lexus bag files to be processed + blue_lexus_red_bag_files = [] + + # Create list of Green Light Workzone Black Pacifica bag files to be processed + black_pacifica_green_bag_files = [] + + # Create list of Green Light Workzone Ford Fusion bag files to be processed + ford_fusion_green_bag_files = [] + + # Create list of Green Light Workzone Blue Lexus bag files to be processed + blue_lexus_green_bag_files = [] + + # Concatenate all Basic Travel bag files into one list + red_light_bag_files = black_pacifica_red_bag_files + ford_fusion_red_bag_files + blue_lexus_red_bag_files + green_light_bag_files = black_pacifica_green_bag_files + ford_fusion_green_bag_files + blue_lexus_green_bag_files + WZ_bag_files = red_light_bag_files + green_light_bag_files + + # Loop to conduct data anlaysis on each bag file: + for bag_file in WZ_bag_files: + print("*****************************************************************") + print("Processing new bag: " + str(bag_file)) + if bag_file in black_pacifica_red_bag_files: + print("Black Pacifica Red Light Workzone Test Case") + elif bag_file in ford_fusion_red_bag_files: + print("Ford Fusion Red Light Workzone Test Case") + elif bag_file in blue_lexus_red_bag_files: + print("Blue Lexus Red Light Workzone Test Case") + if bag_file in black_pacifica_green_bag_files: + print("Black Pacifica Green Light Workzone Test Case") + elif bag_file in ford_fusion_green_bag_files: + print("Ford Fusion Green Light Workzone Test Case") + elif bag_file in blue_lexus_green_bag_files: + print("Blue Lexus Green Light Workzone Test Case") + + # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): + #sys.stdout = orig_stdout + print("Processing bag file " + str(bag_file) + " (" + str(WZ_bag_files.index(bag_file) + 1) + " of " + str(len(WZ_bag_files)) + ")") + #sys.stdout = text_log_file_writer + + # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file + try: + print("Starting To Process Bag at " + str(datetime.datetime.now())) + bag_file_path = str(source_folder) + "/" + bag_file + bag = rosbag.Bag(bag_file_path) + print("Finished Processing Bag at " + str(datetime.datetime.now())) + except: + print("Skipping " + str(bag_file) +", unable to open or process bag file.") + continue + + # Get the rosbag times associated with entering and exiting the active geofence + print("Getting geofence times at " + str(datetime.datetime.now())) + time_enter_geofence, time_exit_geofence, found_geofence_times = get_geofence_entrance_and_exit_times(bag) + print("Got geofence times at " + str(datetime.datetime.now())) + if (not found_geofence_times): + print("Could not find geofence entrance and exit times in bag file.") + continue + + # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test + print("Getting engagement times at " + str(datetime.datetime.now())) + time_test_start_engagement, time_test_end_engagement, found_test_times = get_test_case_engagement_times(bag, time_enter_geofence, time_exit_geofence) + print("Got engagement times at " + str(datetime.datetime.now())) + if (not found_test_times): + print("Could not find test case engagement start and end times in bag file.") + continue + + # Debug Statements + print("Engagement starts at " + str(time_test_start_engagement.to_sec())) + print("Entered Geofence at " + str(time_enter_geofence.to_sec())) + print("Exited Geofence at " + str(time_exit_geofence.to_sec())) + print("Engagement ends at " + str(time_test_end_engagement.to_sec())) + print("Time spent in geofence: " + str((time_exit_geofence - time_enter_geofence).to_sec()) + " seconds") + print("Time spent engaged: " + str((time_test_end_engagement - time_test_start_engagement).to_sec()) + " seconds") + + original_speed_limit = get_route_original_speed_limit(bag, time_test_start_engagement) # Units: m/s + print("Original Speed Limit is " + str(original_speed_limit) + " m/s") + + # Update the exit geofence time to be based off of /guidance/route_state for improved accuracy + time_exit_geofence = adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit) + print("Adjusted geofence exit time (based on /guidance/route_state): " + str(time_exit_geofence)) + + print_lanelet_entrance_times(bag, time_test_start_engagement) + + downtrack_enter_geofence = get_geofence_entrance_downtrack(bag, time_enter_geofence) + + # Initialize results + wz_1_result = None + wz_2_result = None + wz_3_result = None + wz_4_result = None + wz_5_result = None + wz_6_result = None + wz_7_result = None + wz_8_result = None + wz_9_result = None + wz_10_result = None + wz_11_result = None + wz_12_result = None + wz_13_result = None + wz_14_result = None + wz_15_result = None + wz_16_result = None + wz_17_result = None + wz_18_result = None + wz_19_result = None + wz_20_result = None + wz_21_result = None + wz_22_result = None + wz_23_result = None + wz_24_result = None + wz_25_result = None + wz_26_result = None + wz_27_result = None + + # Metrics WZ-4, WZ-5, WZ-6, WZ-7, and WZ-8 + advisory_speed_limit, time_first_msg_received, wz_4_result, wz_5_result, wz_6_result, wz_7_result, wz_8_result = get_workzone_TCM_data(bag) + + # Convert advisory speed limit from WZ-19 to m/s for future metric evaluations + advisory_speed_limit = advisory_speed_limit * 0.44704 # Conversion from mph to m/s + + lanelets_in_geofence = get_geofence_lanelets(bag, time_enter_geofence, advisory_speed_limit) + + # Metric WZ-1 and WZ-5 + closed_lanelets = [12459, 1245999, 1245998] + wz_1_result, wz_5_result = check_geofence_in_initial_route(bag, closed_lanelets) + + # Metric WZ-2 + wz_2_result = check_steady_state_before_first_received_message(bag, time_test_start_engagement, time_first_msg_received, original_speed_limit) + + # Metric WZ-9 + wz_9_result = check_percentage_successful_spat_msg(bag, time_test_start_engagement, time_test_end_engagement) + + # Metric WZ-10 + wz_10_result = check_duration_between_spat_msg_below_max_duration(bag, time_test_start_engagement, time_test_end_engagement) + + stop_bar_location = [-36.8063, 323.251] # Map [x,y] coordinate of stop bar. Hardcoded; same value for every test + #end_geofence_location = [0,0] # Map [x,y] coordinate of the end of the geofence. Hardcoded; same value for every test + + if bag_file in red_light_bag_files: + wz_11_result, time_last_accel, time_stopped = check_deceleration_for_red_light(bag, time_test_start_engagement) + + dist_rear_axle_to_front_bumper = 4.0 # TODO: Obtain improved measurement for this distance for each vehicle + wz_12_result = check_stop_location_for_red_light(bag, stop_bar_location, dist_rear_axle_to_front_bumper, time_test_start_engagement, time_last_accel, time_stopped) + #generate_speed_plot(bag) + + wz_13_result = check_acceleration_time_after_green_light(bag, time_stopped) + + wz_14_result = check_acceleration_after_stop(bag, time_stopped) + + print("WZ-15 N/A (Red Light Bag)") + print("WZ-16 N/A (Red Light Bag)") + + else: + print("WZ-11 N/A (Green Light Bag)") + print("WZ-12 N/A (Green Light Bag)") + print("WZ-13 N/A (Green Light Bag)") + print("WZ-14 N/A (Green Light Bag)") + + + wz_15_result, time_vehicle_at_stop_bar = check_vehicle_speed_at_green_light(bag, advisory_speed_limit, stop_bar_location, time_test_start_engagement) + + wz_16_result = check_deceleration_after_green_light(bag, time_vehicle_at_stop_bar, time_test_start_engagement) + + wz_23_result = check_steady_state_after_geofence(bag, time_test_end_engagement, original_speed_limit) + + wz_24_result = check_speed_limit_when_not_in_geofence(bag, original_speed_limit) + + wz_25_result = check_advisory_speed_limit(bag, advisory_speed_limit, original_speed_limit) + + # Get vehicle type that this bag file is from + vehicle_name = "Unknown" + if bag_file in black_pacifica_green_bag_files or bag_file in black_pacifica_red_bag_files: + vehicle_name = "Black Pacifica" + elif bag_file in ford_fusion_green_bag_files or bag_file in ford_fusion_red_bag_files: + vehicle_name = "Ford Fusion" + else: + vehicle_name = "N/A" + + # Get test type that this bag file is for + vehicle_role = "Unknown" + if bag_file in red_light_bag_files: + vehicle_role = "Red Light" + elif bag_file in green_light_bag_files: + vehicle_role = "Green Light" + + # Write simple pass/fail results to .csv file for appropriate row: + #csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, + # wz_1_result, wz_2_result, wz_3_result, wz_4_result, wz_5_result, wz_6_result, wz_7_result, + # wz_8_result, wz_9_result, wz_10_result, wz_11_result, wz_12_result, wz_13_result, wz_14_result, + # wz_15_result, wz_16_result, wz_17_result, wz_18_result, wz_19_result, wz_20_result, + # wz_21_result, wz_22_result, wz_23_result, wz_24_result, wz_25_result]) + + #sys.stdout = orig_stdout + #text_log_file_writer.close() + return + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/engineering_tools/reindex_active_rosbags.py b/engineering_tools/reindex_active_rosbags.py new file mode 100644 index 0000000000..2550ceed3b --- /dev/null +++ b/engineering_tools/reindex_active_rosbags.py @@ -0,0 +1,81 @@ +#!/usr/bin/python3 + +# Copyright (C) 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. + +import os +import subprocess +import sys + +# Usage: +# python reindex_active_rosbags.py + +def reindex_bag_files(source_folder): + # List of bag files to re-index + # Note: This example list includes .bag.active files from TIM use case testing 5/25-5/27 + files_to_reindex = ["_2021-05-25-17-47-01.bag.active", + "_2021-05-25-17-53-47.bag.active", + "_2021-05-25-18-14-12.bag.active", + "_2021-05-25-18-19-06.bag.active", + "_2021-05-25-18-24-50.bag.active", + "_2021-05-25-18-54-32.bag.active", + "_2021-05-25-19-06-02.bag.active", + "_2021-05-25-19-19-45.bag.active", + "_2021-05-25-20-32-22.bag.active", + "_2021-05-25-20-49-05.bag.active", + "_2021-05-26-13-09-39.bag.active", + "_2021-05-26-13-33-10.bag.active", + "_2021-05-26-17-26-06.bag.active", + "_2021-05-26-19-45-45.bag.active", + "_2021-05-27-20-34-31.bag.active"] + + # Logic to re-index bag files + for filename in files_to_reindex: + print("Reindexing " + str(filename)) + + # Issue command to re-index the bag file + # References example from https://github.com/bierschi/reindexBags + command = "rosbag reindex " + str(filename) + reindex_proc = subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, cwd=source_folder, + executable='/bin/bash') + reindex_proc.wait() + + # If a .orig.active file was created, then the .bag.active file was re-indexed + temp_file_name = str(filename[:-7]) + ".orig.active" + if (temp_file_name in os.listdir(source_folder)): + + # Not removing the .bag.orig.active file for now, can delete manually afterwards in case the re-indexed file is corrupted + #os.remove(temp_file_name) + + # Rename the re-indexed .bag.active to '.bag' only + os.rename(filename, filename[:-7]) + + # Print finished statement + print("Finished re-indexing " + str(filename)) + + return + +def main(): + if len(sys.argv) < 2: + print("Need 1 arguments: process_bag.py ") + exit() + + source_folder = sys.argv[1] + + reindex_bag_files(source_folder) + + return + +if __name__ == "__main__": + main() \ No newline at end of file From 0cb47ede68b6a9d3f8558de008542809e1d68c40 Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Mon, 1 Aug 2022 20:48:32 -0400 Subject: [PATCH 046/165] Update config.yml --- .circleci/config.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.circleci/config.yml b/.circleci/config.yml index fb7d2e2645..c320b264b6 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -92,6 +92,12 @@ jobs: source ${ROS_2_ENV} bash make_with_coverage.bash -t -e /opt/carma/ -o ./coverage_reports/gcov ls -la /opt/carma/coverage_reports/gcov/ + - run: + name: Save Java Test results + command: | + mkdir -p ~/junit + find . -type f -regex ".*/build/test-results/.*xml" -exec cp {} ~/junit/ \; + find . -type f -regex ".*/build/Testing/.*xml" -exec cp {} ~/junit/ \; # Run SonarCloud analysis # PR Branches and number extracted from Circle variables and github api From a27050342a37893968917d8a9057b73848aaa86b Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Mon, 1 Aug 2022 20:49:29 -0400 Subject: [PATCH 047/165] Update config.yml --- .circleci/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index c320b264b6..481666a00c 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -92,7 +92,7 @@ jobs: source ${ROS_2_ENV} bash make_with_coverage.bash -t -e /opt/carma/ -o ./coverage_reports/gcov ls -la /opt/carma/coverage_reports/gcov/ - - run: + - run: name: Save Java Test results command: | mkdir -p ~/junit From 79bfb64281306c5c9b482239486f4cea42b5b92e Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Tue, 2 Aug 2022 17:05:53 -0400 Subject: [PATCH 048/165] Update sonar-scanner.properties --- .sonarqube/sonar-scanner.properties | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 9d8a9e45e2..dd3f8e862b 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -21,8 +21,9 @@ sonar.cfamily.threads=4 sonar.host.url=https://sonarcloud.io sonar.sources=src/main sonar.tests=src/test -#sonar.cfamily.gcov.reportsPath=/opt/carma/coverage_reports/gcov -sonar.coverageReportPaths=/opt/carma/coverage_reports/gcov/coverage.xml +sonar.cfamily.gcov.reportsPath=/opt/carma/coverage_reports/gcov +sonar.coverageReportPaths=/opt/carma/coverage_reports/gcov/coverage01.html +sonar.cfamily.vscoveragexml.reportsPath=/opt/carma/coverage_reports/gcov/coverage.xml # Set Git as SCM sensor sonar.scm.disabled=false sonar.scm.enabled=true From 0c5e07624ae35d81f6727f861771fe69645bcbfd Mon Sep 17 00:00:00 2001 From: SaikrishnaBairamoni Date: Tue, 2 Aug 2022 21:26:00 -0400 Subject: [PATCH 049/165] updated docker files to point develop --- .circleci/config.yml | 2 +- Dockerfile | 2 +- docker/checkout.bash | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index f777779c2d..30644d0592 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -28,7 +28,7 @@ jobs: # Pull docker image from docker hub # XTERM used for better catkin_make output docker: - - image: usdotfhwastol/autoware.ai:carma-system-4.2.0 + - image: usdotfhwastoldev/autoware.ai:develop user: carma environment: TERM: xterm diff --git a/Dockerfile b/Dockerfile index 559b61a25e..5d73cbe514 100644 --- a/Dockerfile +++ b/Dockerfile @@ -33,7 +33,7 @@ # Stage 1 - Acquire the CARMA source as well as any extra packages # ///////////////////////////////////////////////////////////////////////////// -FROM usdotfhwastol/autoware.ai:carma-system-4.2.0 AS base-image +FROM usdotfhwastoldev/autoware.ai:develop AS base-image FROM base-image AS source-code diff --git a/docker/checkout.bash b/docker/checkout.bash index 2d5ffad43b..ef0ace9e87 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -45,9 +45,9 @@ if [[ "$BRANCH" = "develop" ]]; then git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch $BRANCH git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch $BRANCH else - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch carma-system-4.2.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch carma-system-4.2.0 - git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch carma-system-4.2.0 + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-msgs.git --branch develop + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-utils.git --branch develop + git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch develop fi # add astuff messages From b2f8bd38f56fcc47aa4aa375a0ce26224b059f2b Mon Sep 17 00:00:00 2001 From: SaikrishnaBairamoni Date: Tue, 2 Aug 2022 21:41:45 -0400 Subject: [PATCH 050/165] updated release notes page --- docs/Release_notes.md | 102 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 101 insertions(+), 1 deletion(-) diff --git a/docs/Release_notes.md b/docs/Release_notes.md index 57342b4183..fee181987b 100644 --- a/docs/Release_notes.md +++ b/docs/Release_notes.md @@ -1,7 +1,107 @@ CARMA Platform Release Notes ---------------------------- -Version 3.11.0, released Feb 3rd, 2021 +Version 4.2.0, released July 29th, 2022 +---------------------------------------- + +**Summary:** +Carma-platform release version 4.2.0 is comprised of three major enhancements. First, Cooperative Traffic Management (CTM) Speed harmonization. Second, Cooperative Lane Follow (CLF) - Platoon Formation, Operation, Dissolution. Third, Cooperative Lane Coordination (CLC), cooperative lane merge. Along with the above enhancements, several bug fixes and CI related enhancements are included in this release. + +Carma Platform: + +Enhancements in this release: + +- Issue 1766: Updated port the Traffic Incident Parser node to ROS2 and updated several subscribers and publishers created within carma_wm_ros2 in order to support publishers being able to re-publish earlier messages to late-joining subscribers. +- Issue 1762: Updated LCI (Light Controlled Intersection) Strategic Plugin so that it uses the scheduled message from the intersection for setting its ET algorithm parameters instead of computing them based on SPAT if the message is available. +- Issue 1793: Updated Carma wm_ctrl timer classes to Ros2 and changed bridge map directions to 2 to 1 in Carma-config. +- Issue 1812: Updated Configuration parameters in Platooning Strategic IHP logic for enable/disable Front& Rear cut-in join functionality. +- Issue 1810: Added port route node to ROS2 and updated Carma environment launch files and Guidance launch files. +- Issue 1826: Added IHP Strategic and Control plugins, Also Implemented the 2nd iteration of the integrated highway prototype which include cut-in rear and front platoon joins whereas Departure from a platoon and speed harmonization is done via Carma-cloud. +Fixes in this release: + +- Issue 1737: Fixed Current J2735 MAP.msg creator which does not work with TFHRC Vector Map.osm where the Center points created by the tool does not align with road center points in OSM map. +- Issue 1794: Fixed Geofence (getAffectedLaneletOrAreas) function such that Carma should not crash if it detected Geofence (getAffectedLaneletOrAreas) with only 1 point. +- Issue 1799: Fixed IHP Control plugin Cmake file to be compatible with Carma platform's Docker build process. +- Issue 1810: This issue includes small fixes after updating port the route node to ROS2 : +o Stop and Wait Plugin incorrectly had the route node as a dependency, this dependency has been removed. +o Topic naming mismatches were fixed for Trajectory Executor, Roadway Objects, and Traffic Incident Parser. +o ROS1 messages and service related to routing (SetActiveRoute.srv, RouteState.msg) had their field names updated to Snake case to match ROS2 message and service definition requirements. This resulted in an update to the field names in the Port Drayage Plugin package. +o The Route Generator Worker class had a mix of Snake case and Camel case function names. This PR has updated all functions to camelCase to match the style used in other Carma-platform classes. +- Issue 1819: Fixed the Crosstrack distances in world model Trackpos object which returned wrong sign when logic in the method assumes a right handed frame. +- Issue 1846: Fixed Motion computation's parameters that are not set in worker class during runtime updates +- Issue 1847: Fixed the Roadway-objects node and Traffic-Incident-parser crash on receipt of semantic map in Cabin release branch. +- Issue 1851: Fixed the platooning gap calculation which does not consider vehicles in between and gap calculation is always assuming the preceding vehicle is the leader. + +Carma-Cloud: + +Enhancements in this release: + +- Issue 31: Implemented IHP2 Speed Harmonization algorithm in which Carma-cloud application listens to incoming traffic control requests (TCRs) from vehicles, and responds with traffic control messages (TCMs) that has the calculated advisory speed using speed harmonization algorithm. +Fixes in this release: +- Issue 27: Fixed Occasional large delay experienced between CARMA Cloud receiving a TCR from V2XHub and CARMA Cloud sending all corresponding TCMs to V2XHub. + +Autoware.ai: + +Enhancements in this release: + +- Issue 221: Added ROS2 support for Autoware build flags. +- Issue 222: Added Carma Utils Timer interfaces into ROS2 so that they can be used in Carma wm ctrl and Localization manager. + +Fixes in this release: + +- Issue 27: Fixed intermittent large delay experienced between CARMA Cloud receiving a TCR from V2XHub and CARMA Cloud sending all corresponding TCMs to V2XHub. + +Version 4.0.3, released May 10th, 2022 +---------------------------------------- + +**Summary:** +Carma-platform release version 4.0.0 is first version that starts of the transition of system to ROS2 with V2X, Object Perception and some driver nodes transitioned to ROS2 and others still using ROS1 with communication enabled using ROS bridge. This release includes feature enhancements in support of the following proof-of-concept applications to demonstrate the following TSMO use cases: +- Cooperative Traffic Signaling (CTS), fixed signal traversal. +- Commercial Motor Vehicle (CMV) - Work Zone +Along with the above enhancements, several bug fixes are included in this release. +Note: V2X Hub release 7.2 includes CARMA streets plugin for following operations: +- Enhancement to receive, decode and forward the Traffic Control Message (TCM) and Mobility Operations Message (MOM) to enable lane restrictions by vehicle type and to record and notify the vehicle acknowledgement of receiving a TCM from infrastructure. + +Carma Platform: + +Enhancements in this release: + +ROS2 - V2X, Object Perception, Drivers: Developed a ROS2 version of CARMA Platform capable of running on the Lexus RX450h with AutonomouStuff Pacmod3. Specifically, this milestone focused on the creation of a hybrid ROS1 and ROS2 system where the V2X, Object Perception, and core drivers are all ported to ROS2. This milestone included the following enhancements and defect fixes: + +- Issue 1687: Updated the SSC Wrapper to support the ROS2 versions of SSC and Lexus Pacmod while preserving the ROS1 version as well. +- Issue 1500: Integrated driver_discovery and health_monitor behavior into subsystem_controllers/driver_controller +- Issue 1645: Launch ROS2 novatel GPS driver +- Issue 1580: ROS2 V2X Stack migration to ROS2 +- Issue 1557: ROS2 Object Perception Stack migration to ROS2 +- Issue 1277: CARMAWeb UI - Logout should issue a shutdown of the platform +- Issue 1689: Twist Filter node ms to mph speed limit +- Issue 1701: UI based remote launch (non-debug) fails to launch containers +- Issue 1703: UI appears to duplicate /system_alert notifications + +Cooperative Traffic Signaling (CTS), fixed signal traversal: Upon receiving Signal Phase and Timing (SPaT) information (fixed plan information), a vehicle plans a maneuver to proceed through the intersection as efficiently as possible, or come to a safe stop if needed. This milestone include the following enhancements: + +- Issue 1587: Signalized Intersection support in world model to handle SAE J2735 MAP and SPaT messages. +- Issue 1528 Feature/signalized intersection – Signalized Intersection regulatory element has been implemented to support the signalized intersection. + +CMV Work Zone – Enhanced features to demonstrate CMV’s interaction with work zones to reduce speeds, adjust its trajectory to change lanes and adhere to lane use restrictions. The CMV will also demonstrate the capability to acknowledge receipt of work zone geofence information which will include the reduced speed and lane use restriction information. + +- PR 1636: Update WMBroadcaster to process received TCM with a restricted lane +- PR 1682 & 1691: CMV broadcast acknowledgement after processing the incoming TCM + +Fixes in this release: + +- Issue 1608: The Web UI does not notify the user when the vehicle has left the route +- Issue 1634: Route is not selectable due to vehicle position not being set. +- Issue 1650: When CLC follows ILC in a trajectory plan, the first CLC TrajectoryPlanPoint has an abnormally high speed + +Carma-Cloud: + +Enhancement in this release: + +- PR 12: CARMA Cloud sets up Lane Use Restriction to add, remove and update a lane use restriction for vehicles in a work zone area. Enhancement also has been made to receive TCM acknowledgement via the REST interface and log it in the logs. + + +Version 3.11.0, released Feb 3rd, 2022 ---------------------------------------- **Summary:** From ed9fed8827ef4d42df9184e3af29e7b798a0b7b0 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Thu, 4 Aug 2022 10:43:23 -0400 Subject: [PATCH 051/165] Removing duplicate nodes in guidance.launch file Not sure exactly how this happened, but there were duplicates of most plugins being launched in guidance.launch. Now corrected. --- carma/launch/guidance.launch | 62 ------------------------------------ 1 file changed, 62 deletions(-) diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 9d167cf810..14abc2e0a8 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -89,23 +89,6 @@ - - - - - - - - - - - - - - - - - @@ -219,51 +202,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 07878ad169c40cdf087d10227502f256acfefaf6 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 5 Aug 2022 05:29:54 -0700 Subject: [PATCH 052/165] Update for streets implementation --- .../include/lci_strategic_plugin/lci_strategic_plugin.h | 2 +- lci_strategic_plugin/src/lci_strategic_plugin.cpp | 9 ++------- .../include/light_controlled_intersection_plugin.h | 2 +- 3 files changed, 4 insertions(+), 9 deletions(-) diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h index db2af91976..67c6428748 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h @@ -742,7 +742,7 @@ class LCIStrategicPlugin //! Cache variables for storing the current intersection state between state machine transitions boost::optional intersection_speed_; boost::optional intersection_end_downtrack_; - std::string light_controlled_intersection_strategy_ = "Carma/light_controlled_intersection"; + std::string light_controlled_intersection_strategy_ = "signalized"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends // TF listenser tf2_ros::Buffer tf2_buffer_; diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index b8ae01e8b0..30e73ebdc7 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -892,18 +892,13 @@ void LCIStrategicPlugin::BSMCb(const cav_msgs::BSMConstPtr& msg) void LCIStrategicPlugin::parseStrategyParams(const std::string& strategy_params) { - // sample strategy_params: "st:1634067044,et:1634067059" + // sample strategy_params: "et:1634067059" std::string params = strategy_params; std::vector inputsParams; boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); - std::vector st_parsed; - boost::algorithm::split(st_parsed, inputsParams[0], boost::is_any_of(":")); - scheduled_stop_time_ = std::stoull(st_parsed[1]); - ROS_DEBUG_STREAM("scheduled_stop_time_: " << scheduled_stop_time_); - std::vector et_parsed; - boost::algorithm::split(et_parsed, inputsParams[1], boost::is_any_of(":")); + boost::algorithm::split(et_parsed, inputsParams[0], boost::is_any_of(":")); scheduled_enter_time_ = std::stoull(et_parsed[1]); ROS_DEBUG_STREAM("scheduled_enter_time_: " << scheduled_enter_time_); diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin.h b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin.h index f15b2041e5..171bd341aa 100755 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin.h +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin.h @@ -213,7 +213,7 @@ class LightControlledIntersectionTacticalPlugin carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_; std::vector last_final_speeds_; - std::string light_controlled_intersection_strategy_ = "Carma/light_controlled_intersection"; + std::string light_controlled_intersection_strategy_ = "signalized"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with }; From e1e2553c508f59ebe36251372ac8f68d44f6348a Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 5 Aug 2022 05:55:47 -0700 Subject: [PATCH 053/165] update tests --- lci_strategic_plugin/test/test_strategic_plugin.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/lci_strategic_plugin/test/test_strategic_plugin.cpp b/lci_strategic_plugin/test/test_strategic_plugin.cpp index 57998cac94..4dccee4be3 100755 --- a/lci_strategic_plugin/test/test_strategic_plugin.cpp +++ b/lci_strategic_plugin/test/test_strategic_plugin.cpp @@ -83,7 +83,7 @@ TEST_F(LCIStrategicTestFixture, planManeuverCb) ASSERT_NEAR(24.01, resp.new_plan.maneuvers[0].lane_following_maneuver.end_time.toSec(), 0.01); ASSERT_NEAR(300, resp.new_plan.maneuvers[0].lane_following_maneuver.end_dist, 0.0001); // check trajectory smoothing parameters: - ASSERT_EQ("Carma/light_controlled_intersection", resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.string_valued_meta_data.front()); + ASSERT_EQ("signalized", resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.string_valued_meta_data.front()); ASSERT_NEAR(0.6823, resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.float_valued_meta_data[0], 0.01); ASSERT_NEAR(-0.6823, resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.float_valued_meta_data[1], 0.01); ASSERT_NEAR(85.00, resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.float_valued_meta_data[2], 0.01); @@ -384,7 +384,7 @@ TEST_F(LCIStrategicTestFixture, handleFailureCase) TEST(LCIStrategicPluginTest, moboperationcbtest) { cav_msgs::MobilityOperation msg; - msg.strategy = "Carma/light_controlled_intersection"; + msg.strategy = "signalized"; std::shared_ptr wm = std::make_shared(); LCIStrategicPluginConfig config; @@ -400,7 +400,7 @@ TEST(LCIStrategicPluginTest, moboperationcbtest) TEST(LCIStrategicPluginTest, parseStrategyParamstest) { cav_msgs::MobilityOperation msg; - msg.strategy_params = "st:16000,et:32000"; + msg.strategy_params = "et:32000"; std::shared_ptr wm = std::make_shared(); LCIStrategicPluginConfig config; @@ -413,7 +413,7 @@ TEST(LCIStrategicPluginTest, parseStrategyParamstest) EXPECT_EQ(32000, lcip.scheduled_enter_time_); cav_msgs::MobilityOperation outgoing_msg = lcip.generateMobilityOperation(); - EXPECT_EQ(outgoing_msg.strategy, "Carma/light_controlled_intersection"); + EXPECT_EQ(outgoing_msg.strategy, "signalized"); EXPECT_EQ(outgoing_msg.m_header.sender_id, config.vehicle_id); std::cout << "strategy_param: " << outgoing_msg.strategy_params << std::endl; } From 4dbf855dd5318d1113442037bffed0ca06882a09 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 5 Aug 2022 05:58:24 -0700 Subject: [PATCH 054/165] remove unset check --- lci_strategic_plugin/src/lci_strategic_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index 30e73ebdc7..21689838ff 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -990,7 +990,7 @@ bool LCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav return true; } - bool is_empty_schedule_msg = (scheduled_stop_time_ == 0 && scheduled_enter_time_ == 0); + bool is_empty_schedule_msg = (scheduled_enter_time_ == 0); if (is_empty_schedule_msg) { resp.new_plan.maneuvers = {}; From ad5baec8e13a05bddee82a67b1313ca2cd748464 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 5 Aug 2022 05:59:17 -0700 Subject: [PATCH 055/165] remove unused variable --- .../include/lci_strategic_plugin/lci_strategic_plugin.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h index 67c6428748..791e76ea89 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h @@ -191,8 +191,6 @@ class LCIStrategicPlugin // CARMA Streets Variakes // timestamp for msg received from carma streets unsigned long long street_msg_timestamp_ = 0; - // scheduled stop time - unsigned long long scheduled_stop_time_ = 0; // scheduled enter time unsigned long long scheduled_enter_time_ = 0; From 97c9497a69d5db8fc83043d72fb6fc40731ba2d4 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 5 Aug 2022 07:15:37 -0700 Subject: [PATCH 056/165] fixing published message --- lci_strategic_plugin/src/lci_strategic_plugin.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index 21689838ff..1788718d2d 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -919,10 +919,9 @@ cav_msgs::MobilityOperation LCIStrategicPlugin::generateMobilityOperation() if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right"; if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left"; - - mo_.strategy_params = "max_accel: " + std::to_string(vehicle_acceleration_limit_) + + mo_.strategy_params = "access: " + "1" + "max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) + - ", min_gap: " + std::to_string(config_.min_gap) + + ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: " + "0" + // NOTE: Departure position set to 0 at all times since it's not specified by streets ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_); From ddff883abadf7aa23cb157a9fcff95e83e86e70e Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 5 Aug 2022 07:18:01 -0700 Subject: [PATCH 057/165] fixing published message --- lci_strategic_plugin/src/lci_strategic_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index 1788718d2d..9b05dee147 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -919,7 +919,7 @@ cav_msgs::MobilityOperation LCIStrategicPlugin::generateMobilityOperation() if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right"; if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left"; - mo_.strategy_params = "access: " + "1" + "max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets + mo_.strategy_params = "access: " + "1" + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) + ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: " + "0" + // NOTE: Departure position set to 0 at all times since it's not specified by streets ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_); From e2181425430ec98814f12d3340f6c665d9368d70 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Fri, 5 Aug 2022 09:18:47 -0700 Subject: [PATCH 058/165] Remove rosbag analysis scripts --- .../analyze_TIM_and_RWM_rosbags.py | 1195 ---------- .../analyze_basic_travel_rosbags.py | 1943 ----------------- .../analyze_freight_workzone_rosbags.py | 1533 ------------- .../data_processing/analyze_ihp2_rosbags.py | 1276 ----------- .../analyze_port_drayage_rosbags.py | 975 --------- .../analyze_tsmo_uc2_rosbags.py | 443 ---- .../analyze_voices_sit1_rosbags.py | 776 ------- .../data_processing/analyze_wz_rosbags.py | 1299 ----------- 8 files changed, 9440 deletions(-) delete mode 100644 engineering_tools/data_processing/analyze_TIM_and_RWM_rosbags.py delete mode 100644 engineering_tools/data_processing/analyze_basic_travel_rosbags.py delete mode 100644 engineering_tools/data_processing/analyze_freight_workzone_rosbags.py delete mode 100644 engineering_tools/data_processing/analyze_ihp2_rosbags.py delete mode 100644 engineering_tools/data_processing/analyze_port_drayage_rosbags.py delete mode 100644 engineering_tools/data_processing/analyze_tsmo_uc2_rosbags.py delete mode 100644 engineering_tools/data_processing/analyze_voices_sit1_rosbags.py delete mode 100644 engineering_tools/data_processing/analyze_wz_rosbags.py diff --git a/engineering_tools/data_processing/analyze_TIM_and_RWM_rosbags.py b/engineering_tools/data_processing/analyze_TIM_and_RWM_rosbags.py deleted file mode 100644 index 35dbd0c873..0000000000 --- a/engineering_tools/data_processing/analyze_TIM_and_RWM_rosbags.py +++ /dev/null @@ -1,1195 +0,0 @@ -#!/usr/bin/python3 - -# Copyright (C) 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. - -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime - -# HOW TO USE SCRIPT: -# Run the following in a terminal to download dependencies: -# sudo add-apt-repository ppa:deadsnakes/ppa -# sudo apt-get update -# sudo apt install python3.7 -# python3.7 -m pip install --upgrade pip -# python3.7 -m pip install matplotlib -# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg -# python3.7 -m pip install lz4 -# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ -# In terminal, navigate to the directory that contains this python script and run the following: -# python3.7 analyze_TIM_test_rosbags.py - -# Helper Function: Get times associated with the system entering the geofence and exiting the geofence -def get_geofence_entrance_and_exit_times(bag): - # Initialize geofence entrance and exit times - time_enter_active_geofence = rospy.Time() - time_exit_active_geofence = rospy.Time() - - # Find geofence entrance and exit times - is_on_active_geofence = False - found_geofence_entrance_time = False - found_geofence_exit_time = False - for topic, msg, t in bag.read_messages(topics=['/environment/active_geofence']): - # If first occurrence of being in the active geofence, set the start time - if (msg.is_on_active_geofence and not is_on_active_geofence): - time_enter_active_geofence = t - #print("Entered geofence at " + str(t.to_sec())) - found_geofence_entrance_time = True - is_on_active_geofence = True - - # If final occurrence of being in the active geofence, set the end time - if (not msg.is_on_active_geofence and is_on_active_geofence): - time_exit_active_geofence = t - found_geofence_exit_time = True - break - - # Check if both geofence start and end time were found - found_geofence_times = False - if (found_geofence_entrance_time and found_geofence_exit_time): - found_geofence_times = True - - return time_enter_active_geofence, time_exit_active_geofence, found_geofence_times - -# Helper Function: Get the original speed limit for the lanelets within the vehicle's route -# Note: Assumes that all lanelets in the route share the same speed limit prior to the first geofence TIM message being processed. -def get_route_original_speed_limit(bag, time_test_start_engagement): - # Initialize the return variable - original_speed_limit = 0.0 - - # Find the speed limit associated with the first lanelet when the system first becomes engaged - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_test_start_engagement): - original_speed_limit = msg.speed_limit - break - - return original_speed_limit - -# Helper function: Begin with the time that the vehicle exits the active geofence according to -# /guidance/active_geofence topic. This adjusts the time to be based on /guidance/route_state -# in order to be more accurate -def adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit): - # Get the true time of the end of the geofence, based on when /guidance/route_state displays the - # original speed limit for the current vehicle location - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence): - if msg.speed_limit == original_speed_limit: - true_time_exit_geofence = t - break - - return true_time_exit_geofence - -# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section -def get_test_case_engagement_times(bag, time_enter_active_geofence, time_exit_active_geofence): - # Initialize system engagement start and end times - time_start_engagement = rospy.Time() - time_stop_engagement = rospy.Time() - - # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section - is_engaged = False - found_test_case_engagement_times = False - has_reached_geofence_entrance = False - has_reached_geofence_exit = False - for topic, msg, t in bag.read_messages(topics=['/guidance/state']): - # If entering engagement, track this start time - if (msg.state == 4 and not is_engaged): - time_start_engagement = t - is_engaged = True - - # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance - # state can be published. - if (msg.state == 4): - time_last_engaged = t - - # If exiting engagement, check whether this period of engagement included the geofence entrance and exit times - elif (msg.state != 4 and is_engaged): - is_engaged = False - time_stop_engagement = t - - # Check if the engagement start time was before the geofence entrance and exit times - if (time_start_engagement <= time_enter_active_geofence and t >= time_enter_active_geofence): - has_reached_geofence_entrance = True - if (time_start_engagement <= time_exit_active_geofence and t >= time_exit_active_geofence): - has_reached_geofence_exit = True - - # Set flag if this engagement period includes both the geofence entrance and exit times - if (has_reached_geofence_entrance and has_reached_geofence_exit): - found_test_case_engagement_times = True - break - - # If CARMA ended engagement before guidance state could be updated, check if the last recorded - # time of engagement came after exiting the geofence - if not found_test_case_engagement_times: - if time_last_engaged >= time_exit_active_geofence: - time_stop_engagement = time_last_engaged - found_test_case_engagement_times = True - - return time_start_engagement, time_stop_engagement, found_test_case_engagement_times - -########################################################################################################### -# TIM B-1: Geofenced area is a part of the initial route plan. -# TIM B-11: The vehicle receives a message from the CM vehicle that includes the closed lane ahead. The -# vehicle processes this closed lane information. -# RWM B-1: Geofenced area is a part of the initial route plan. -# RWM B-11: The vehicle receives a message from CC that includes the closed lane ahead. The vehicle processes -# this closed lane information. -########################################################################################################### -def check_geofence_route_metrics(bag, closed_lanelets): - # Get each set route from the bag file (includes set routes and updated/re-rerouted routes) - shortest_path_lanelets = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route']): - # Print as Debug Statement - print("Shortest Path Route Update at " + str(t.to_sec()) + ": " + str(msg.shortest_path_lanelet_ids)) - - shortest_path_lanelets.append([]) - for lanelet_id in msg.shortest_path_lanelet_ids: - shortest_path_lanelets[-1].append(lanelet_id) - - # If there are two route paths, check that the first (original) route contains the closed lanelet(s) and the second route doesn't - # Note: Assumes there should be only two routes: (1) the initial route and (2) the re-routed route - initial_route_includes_closed_lane = False # Flag for B-1 success - map_is_updated_for_closed_lane = False # Flag for B-11 success - if (len(shortest_path_lanelets) == 2): - original_shortest_path = shortest_path_lanelets[0] - rerouted_shortest_path = shortest_path_lanelets[1] - - for lanelet_id in closed_lanelets: - if lanelet_id in original_shortest_path: - initial_route_includes_closed_lane = True - else: - initial_route_includes_closed_lane = False - break - - for lanelet_id in closed_lanelets: - if lanelet_id not in rerouted_shortest_path: - map_is_updated_for_closed_lane = True - else: - map_is_updated_for_closed_lane = False - break - else: - print("Invalid quantity of route updates found in bag file (" + str(len(shortest_path_lanelets)) + " found, 2 expected)") - - # Print result statements and return success flags - if (initial_route_includes_closed_lane): - print("B-1 succeeded; all closed lanelets " + str(closed_lanelets) + " were in the initial route") - else: - print("B-1 failed: not all closed lanelets " + str(closed_lanelets) + " were in the initial route.") - - if (map_is_updated_for_closed_lane): - print("B-11 succeeded: no closed lanelets " + str(closed_lanelets) + " were in the re-routed route.") - else: - print("B-11 failed: at least 1 closed lanelet " + str(closed_lanelets) + " was in the re-routed route.") - - return initial_route_includes_closed_lane, map_is_updated_for_closed_lane - -########################################################################################################### -# TIM B-10: The vehicle receives a message from the CM vehicle that includes the new speed limit ahead. -# The vehicle processes this new speed limit. -# RWM B-10: The vehicle receives a message from CARMA Cloud that includes the new speed limit ahead. -# The vehicle processes this new speed limit. -########################################################################################################### -def check_in_geofence_speed_limits(bag, time_enter_geofence, time_exit_geofence, advisory_speed_limit): - # Check that a TrafficControlMessage was published using the correct advisory speed limit - advisory_speed_limit_mph = advisory_speed_limit / 0.44704 # Conversion from m/s to mph - has_communicated_advisory_speed_limit = False - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - if (msg.tcmV01.params.detail.choice == 12 and msg.tcmV01.params.detail.maxspeed == advisory_speed_limit_mph): - has_communicated_advisory_speed_limit = True - break - elif (msg.tcmV01.params.detail.choice == 12 and msg.tcmV01.params.detail.maxspeed != advisory_speed_limit_mph): - has_communicated_advisory_speed_limit = False - break - - epsilon = 0.01 - has_correct_geofence_lanelet_speed_limits = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_enter_geofence, end_time = time_exit_geofence): - # Failure if the current lanelet is one of the geofence lanelets and its speed limit doesn't match the advisory speed limit - if (abs(msg.speed_limit - advisory_speed_limit) >= epsilon): - print("Lanelet ID " + str(msg.lanelet_id) + " has speed limit of " + str(msg.speed_limit) + " m/s. " + \ - "Does not match advisory speed limit of " + str(advisory_speed_limit) + " m/s.") - has_correct_geofence_lanelet_speed_limits = False - break - - if (has_communicated_advisory_speed_limit and has_correct_geofence_lanelet_speed_limits): - print("B-10 succeeded; system processed an advisory speed limit of " + str(advisory_speed_limit) + " m/s") - is_successful = True - else: - print("B-10 failed; system did not process an advisory speed limit of " + str(advisory_speed_limit) + " m/s") - is_successful = False - - return is_successful - -########################################################################################################### -# RWM B-28: The information communicated by CC is closed area, what the new speed limit is in the closed -# area, and what the minimum gap limits are in the closed area. -########################################################################################################### -def get_RWM_TCM_data(bag): - # Check that TCM Messages are received for closed lane, max speed, and minimum headway (minimum gap) - has_closed_lane = False - has_advisory_speed = False - has_minimum_gap = False - minimum_gap = 0.0 - advisory_speed = 0.0 - time_first_msg_received = rospy.Time() - is_successful = False - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - time_first_msg_received = t - if msg.tcmV01.params.detail.choice == 5 and msg.tcmV01.params.detail.closed == 1: - has_closed_lane = True - elif msg.tcmV01.params.detail.choice == 13: - has_minimum_gap = True - minimum_gap = msg.tcmV01.params.detail.minhdwy - elif msg.tcmV01.params.detail.choice == 12: - has_advisory_speed = True - advisory_speed = msg.tcmV01.params.detail.maxspeed - - if has_closed_lane and has_advisory_speed and has_minimum_gap: - print("TCM Messages Received: Closed Lane; Advisory Speed: " + str(advisory_speed) + \ - " mph; Minimum Gap: " + str(minimum_gap) + " meters") - is_successful = True - break - - # Print out route state for each new lanelet: - id = 0 - print("/guidance/route_state lanelet change times:") - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): - if msg.lanelet_id != id: - - print("Time: " + str(t.to_sec()) + "; Lanelet: " + str(msg.lanelet_id) + "; Speed Limit: " + str(msg.speed_limit)) - id = msg.lanelet_id - - - return minimum_gap, advisory_speed, time_first_msg_received, is_successful - -########################################################################################################### -# TIM B-28: The information communicated by the CM vehicle is closed area, and what the new -# speed limit and gap limits are in the closed area. -########################################################################################################### -# Example Params: "lat:39.23371506,lon:-77.96981812,downtrack:1,uptrack:80,min_gap:6, -# advisory_speed:15,event_reason:MOVE OVER LAW,event_type:CLOSED" -def get_TIM_mobility_operation_data(bag): - # Initialize the return variables - minimum_gap = 0.0 - advisory_speed = 0.0 - event_type = "" - time_first_msg_received = rospy.Time() - has_correct_data = False - - # Parse the first received TIM use case Mobility Operation message - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_operation']): - #print(msg.strategy) - if (msg.strategy == "carma3/Incident_Use_Case"): - time_first_msg_received = t - print("Received carma3/Incident_Use_Case strategy_params: " + str(msg.strategy_params)) - - # Parse the strategy_params string: - params = (msg.strategy_params).split(',') - - minimum_gap_text = params[4].split(':') - advisory_speed_text = params[5].split(':') - event_type_text = params[7].split(':') - - if (minimum_gap_text[0] == "min_gap"): - minimum_gap = float(minimum_gap_text[1]) - has_correct_data = True - else: - has_correct_data = False - break - - if (advisory_speed_text[0] == "advisory_speed"): - advisory_speed = float(advisory_speed_text[1]) - has_correct_data = True - else: - has_correct_data = False - break - - if (event_type_text[0] == "event_type" and event_type_text[1] == "CLOSED"): - event_type = event_type_text[1] - has_correct_data = True - else: - has_correct_data = False - break - - # Only parse the first TIM Mobility Operation message - break - - if (has_correct_data): - print("B-28 succeeded; TIM MobilityOperation message received with advisory speed limit " + str(advisory_speed) + " mph, " \ - + "minimum gap " + str(minimum_gap) + " meters, and \"CLOSED\" event type.") - is_successful = True - else: - print("B-28 failed; no TIM MobilityOperation message received with all required information (advisory speed limit, minimum gap, " \ - + " and \"CLOSED\" event type.") - is_successful = False - - return minimum_gap, advisory_speed, event_type, time_first_msg_received, is_successful - -########################################################################################################### -# TIM B-2: Amount of time that the vehicle is going at a steady state (e.g. same lane, constant speed) -# before it receives any messages. -# RWM B-2: Amount of time that the vehicle is going at steady state (e.g. same lane, constant speed) before -# it receives the first CC message. -########################################################################################################### -def check_steady_state_before_first_received_message(bag, time_start_engagement, time_received_first_message, original_speed_limit): - # (m/s) Threshold offset of vehicle speed to speed limit to be considered at steady state - threshold_speed_limit_offset = 0.89408 # 0.89408 m/s is 1 mph - min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset - max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset - - # (seconds) Minimum time between vehicle reaching steady state and first TIM MobilityOperation message being received - min_time_between_steady_state_and_msg = 10.0 - - # Get the time that the vehicle reaches within the set offset of the speed limit (while system is engaged) - time_start_steady_state = 0.0 - has_reached_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement): - current_speed = msg.twist.linear.x # Current vehicle speed in m/s - if (max_steady_state_speed >= current_speed >= min_steady_state_speed): - has_reached_steady_state = True - time_start_steady_state = t - break - - # Check if the time the vehicle reaches steady state is more than 'min_time_between_steady_state_and_msg' seconds before the first received TIM message - if (has_reached_steady_state): - time_between_steady_state_and_msg = (time_received_first_message - time_start_steady_state).to_sec() - if (time_between_steady_state_and_msg >= min_time_between_steady_state_and_msg): - is_successful = True - print("B-2 succeeded; reached steady state " + str(time_between_steady_state_and_msg) + " seconds before receiving first TIM or TCM message.") - else: - is_successful = False - if (time_between_steady_state_and_msg > 0): - print("B-2 failed; reached steady state " + str(time_between_steady_state_and_msg) + " seconds before receiving first TIM or TCM message.") - else: - print("B-2 failed; reached steady state " + str(-time_between_steady_state_and_msg) + " seconds after receiving first TIM or TCM message.") - else: - print("B-2 failed; vehicle never reached steady state during rosbag recording.") - is_successful = False - - return is_successful - -########################################################################################################### -# TIM B-13: Ending point of the trajectory associated with lane merging, relative to the beginning of the geofence. -# RWM B-13: Ending point of the trajectory associated with lane merging, relative to the beginning of the geofence. -########################################################################################################### -def check_lane_merge_before_geofence(bag, time_start_engagement, time_enter_geofence): - # (feet) Maximum distance that lane merge trajectory can end before geofence entrance - max_distance_from_geofence = 50.0 - # (feet) Minimum distance that lane merge trajectory can end before geofence entrance - min_distance_from_geofence = 0.0 - - """ - # DEBUG Statements: Maneuver Plan Output - for topic, msg, t in bag.read_messages(topics=['/guidance/final_maneuver_plan'], start_time = time_start_engagement, end_time = time_enter_geofence): - print("************************") - print("Maneuvers in plan: " + str(len(msg.maneuvers))) - for maneuver in msg.maneuvers: - if (maneuver.type == 0): - print("Lane Keeping: " + str(maneuver.lane_following_maneuver.start_time) + " to " + str(maneuver.lane_following_maneuver.end_time)) - print("Speed: " + str(maneuver.lane_following_maneuver.start_speed) + " to " + str(maneuver.lane_following_maneuver.end_speed)) - print("Distance: " + str(maneuver.lane_following_maneuver.start_dist) + " to " + str(maneuver.lane_following_maneuver.end_dist)) - print("Lanelet: " + str(maneuver.lane_following_maneuver.lane_id)) - elif (maneuver.type == 1): - print("Lane Change: " + str(maneuver.lane_change_maneuver.start_time) + " to " + str(maneuver.lane_change_maneuver.end_time)) - print("Distance: " + str(maneuver.lane_change_maneuver.start_dist) + " to " + str(maneuver.lane_change_maneuver.end_dist)) - print("Speed: " + str(maneuver.lane_change_maneuver.start_speed) + " to " + str(maneuver.lane_change_maneuver.end_speed)) - print("Lanelet: " + str(maneuver.lane_change_maneuver.starting_lane_id) + " to " + str(maneuver.lane_change_maneuver.ending_lane_id)) - elif (maneuver.type == 5): - print("StopAndWait: " + str(maneuver.stop_and_wait_maneuver.start_time) + " to " + str(maneuver.stop_and_wait_maneuver.end_time)) - """ - - # Get the location (in Map Frame) of the start of the geofence - for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_enter_geofence): - start_geofence_x = msg.pose.position.x - start_geofence_y = msg.pose.position.y - break - - # For each trajectory plan (prior to entering the geofence) that contains the end of a lane-change, check that - # the point associated with the end of that lane merge is 0-50 feet prior to the beginning of the geofence: - # Note: Currently assumes the only lane change prior to the geofence is the one immediately before the start of the geofence - total_distance_from_geofence = 0.0 # feet - count_success_lane_merge_point = 0 - count_fail_lane_merge_point = 0 - for topic, msg, t in bag.read_messages(topics=['/guidance/plan_trajectory'], start_time = time_start_engagement, end_time = time_enter_geofence): - #print("*************************************************") - total_distance = 0.0 - has_found_lane_merge_end_point = False - prev_point = msg.trajectory_points[0] - for i in range(1, len(msg.trajectory_points)): - current_point = msg.trajectory_points[i] - - # Calculations for Debug Statement - distance_between_lane_change_points = ((msg.trajectory_points[i].x - prev_point.x)**2 + (msg.trajectory_points[i].y - prev_point.y)**2) ** 0.5 - dt = msg.trajectory_points[i].target_time - prev_point.target_time - if (dt.to_sec() <= 0.001): - prev_point = current_point - continue - current_pt_speed = distance_between_lane_change_points / dt.to_sec() # Speed in m/s - total_distance += distance_between_lane_change_points - #print(str(current_point.planner_plugin_name) + ": " + str(current_point.target_time) + ", speed: " + str(current_pt_speed) + " m/s, distance: " +str(total_distance)) - - - # If previous point was planned by a lane change plugin and current point was not, the previous point was the final lane merge point - if ((prev_point.planner_plugin_name == "UnobstructedLaneChangePlugin" or prev_point.planner_plugin_name == "CooperativeLaneChangePlugin") and \ - (current_point.planner_plugin_name != "UnobstructedLaneChangePlugin" and current_point.planner_plugin_name != "CooperativeLaneChangePlugin")): - - # Get end of lane merge coordinates (In Map Frame) - end_lane_merge_x = prev_point.x - end_lane_merge_y = prev_point.y - has_found_lane_merge_end_point = True - - # Otherwise, if current point was planned by a lane change plugin and is the last trajectory plan point, it is the final lane merge point - elif ((current_point.planner_plugin_name == "UnobstructedLaneChangePlugin" or current_point.planner_plugin_name == "CooperativeLaneChangePlugin") and \ - i == len(msg.trajectory_points) - 1): - - # Get end of lane merge coordinates (In Map Frame) - end_lane_merge_x = current_point.x - end_lane_merge_y = current_point.y - has_found_lane_merge_end_point = True - - # Evaluate the distance from the end of the lane merge to the start of the geofence if the end point has been found - if (has_found_lane_merge_end_point): - distance_to_geofence_meters = ((end_lane_merge_x - start_geofence_x)**2 + (end_lane_merge_y - start_geofence_y)**2)**0.5 # In meters - distance_to_geofence_feet = distance_to_geofence_meters * 3.28 # Conversion from meters to feet - - if (max_distance_from_geofence >= distance_to_geofence_feet >= min_distance_from_geofence): - count_success_lane_merge_point += 1 - #print("Successful lane-merge trajectory; ends " + str(distance_to_geofence_meters) + " meters before geofence.") - else: - count_fail_lane_merge_point += 1 # Point was not on a preceding lanelet - #print("Failed lane-merge trajectory; ends " + str(distance_to_geofence_meters) + " meters before geofence.") - - total_distance_from_geofence += distance_to_geofence_feet - break - - # Update previous point - prev_point = msg.trajectory_points[i] - - """ - # Check that the lane merge end point is within 0-50 feet before geofence start AND in a preceding lanelet - # TODO: Add this logic back in once route state bug is fixed and the current lanelet can be identified - if (max_distance_from_geofence >= distance_to_geofence_feet >= min_distance_from_geofence): - end_lane_merge_lanelet_id = tpp.lane_id - - # Check that the end-of-lane-merge lanelet ID precedes the start-of-geofence lanelet ID in the shortest path: - for i in range(0, len(rerouted_shortest_path)): - if (end_lane_merge_lanelet_id == rerouted_shortest_path[i]): - end_lane_merge_idx = i - if (start_geofence_lanelet_id == rerouted_shortest_path[i]): - start_geofence_idx = i - - if (end_lane_merge_idx < start_geofence_idx): - # TODO: Track the successful end-of-lane-merge points - count_success_lane_merge_point += 1 - print("Successful lane-merge trajectory; ends " + str(distance_to_geofence_meters) + " meters before geofence.") - else: - # TODO: Track the unsuccessful end-of-lane-merge points - count_fail_lane_merge_point += 1 # Point was not on a preceding lanelet - print("Failed lane-merge trajectory; ends " + str(distance_to_geofence_meters) + " meters before geofence.") - """ - - count_traj_plans_with_lane_merge = count_success_lane_merge_point + count_fail_lane_merge_point - - if count_traj_plans_with_lane_merge > 0: - average_distance_from_geofence = total_distance_from_geofence / float(count_traj_plans_with_lane_merge) - - is_successful = False - if (count_traj_plans_with_lane_merge == 0): - print("B-13 failed; found no trajectories with a lane merge prior to entering the geofence.") - is_successful = False - elif (count_fail_lane_merge_point == 0): - print("B-13 succeeded; succeeded on " + str(count_success_lane_merge_point) + " of " + str(count_traj_plans_with_lane_merge) + " lane-merge trajectory plans." \ - + " Average lane merge end point distance from start of geofence was " + str(average_distance_from_geofence) + " feet.") - is_successful = True - else: - print("B-13 failed; succeeded on " + str(count_success_lane_merge_point) + " of " + str(count_traj_plans_with_lane_merge) + " lane-merge trajectory plans." \ - + " Average lane merge end point distance from start of geofence was " + str(average_distance_from_geofence) + " feet.") - is_successful = False - - return is_successful - -########################################################################################################### -# TIM B-15: The actual trajectory to prepare for the geofence will include a deceleration section and -# the average deceleration amount shall be no less than 1 m/s^2. -# RWM B-15: The actual trajectory to prepare for the geofence will include a deceleration section and -# the average deceleration amount shall be no less than 1 m/s^2. -########################################################################################################### -def check_deceleration_before_geofence(bag, time_enter_geofence, original_speed_limit, advisory_speed_limit): - # Parameters used for metric evaluation - min_average_deceleration = 1.0 # m/s^2 - start_decel_percent_of_original_speed_limit = 0.90 # Percentage (0.90 is 90%) of original speed limit for current speed to be considered the start of deceleration - end_decel_percent_of_advisory_speed_limit = 1.10 # Percentage (1.10 is 110%) of advisory speed limit for current speed to be considered the end of deceleration - - # Variables to track during metric evaluation - speed_start_decel = 0.0 - speed_end_decel = 0.0 - time_start_decel = rospy.Time() - time_end_decel = rospy.Time() - has_found_start_of_decel = False - has_found_end_of_decel = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_enter_geofence): - if has_found_start_of_decel: - decel = (msg.twist.linear.x - prev_speed) / (t-prev_time).to_sec() - #print("Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x) + "; " + str(decel) + " m/s^2") - prev_speed = msg.twist.linear.x - prev_time = t - - # Get start time of deceleration - if not has_found_start_of_decel and (msg.twist.linear.x < (start_decel_percent_of_original_speed_limit * original_speed_limit)): - print("Start Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_start_decel = t - speed_start_decel = msg.twist.linear.x - has_found_start_of_decel = True - prev_speed = msg.twist.linear.x - prev_time = t - - # Get end time of deceleration - if has_found_start_of_decel and (msg.twist.linear.x < (end_decel_percent_of_advisory_speed_limit * advisory_speed_limit)): - print("End Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_end_decel = t - speed_end_decel = msg.twist.linear.x - has_found_end_of_decel = True - break - - deceleration_completed_before_geofence = False - if (time_enter_geofence-time_end_decel).to_sec() > 0: - deceleration_completed_before_geofence = True - - is_successful = False - if has_found_start_of_decel and has_found_end_of_decel: - - # Calculate the average deceleration rate during the deceleration phase - average_deceleration = (speed_start_decel - speed_end_decel) / (time_end_decel - time_start_decel).to_sec() - - if average_deceleration >= min_average_deceleration and deceleration_completed_before_geofence: - print("B-15 succeeded; average deceleration above 1.0 m/s^2 occurred before geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = True - elif average_deceleration >= min_average_deceleration and not deceleration_completed_before_geofence: - print("B-15 failed; average deceleration above 1.0 m/s^2 occurred after geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = False - elif average_deceleration <= min_average_deceleration and deceleration_completed_before_geofence: - print("B-15 failed; a deceleration below 1.0 m/s^2 occurred before the geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = False - elif average_deceleration <= min_average_deceleration and not deceleration_completed_before_geofence: - print("B-15 failed; a deceleration below 1.0 m/s^2 occurred after the geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = False - elif has_found_start_of_decel: - print("B-15 failed; did not find end of deceleration phase") - is_successful = False - else: - print("B-15 failed; did not find start of deceleration phase") - is_successful = False - - return is_successful - -########################################################################################################### -# TIM B-20: After leaving the geofenced area, the planned trajectory for a lane change back to the -# original lane will start within the first 30 feet. -# RWM B-20: After leaving the geofenced area, the planned trajectory for a lane change back to the -# original lane will start within the first 30 feet. -########################################################################################################### -def check_lane_change_after_geofence(bag, time_exit_geofence): - max_distance_from_geofence_end = 30.0 # (feet) Max distance from end of geofence for first lane change point - min_distance_from_geofence_end = 0.0 # (feet) Minimum distance from end of geofence for first lane change point - is_successful = False - - # Get the location (in Map Frame) of the end of the geofence - for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_exit_geofence): - exit_geofence_x = msg.pose.position.x - exit_geofence_y = msg.pose.position.y - break - - # Get the position of the first planned lane change trajectory plan point after exiting the geofence (if one exists) - has_lane_change = False - for topic, msg, t in bag.read_messages(topics=['/guidance/plan_trajectory'], start_time = time_exit_geofence): - # Get the first lane change point's position - for tpp in msg.trajectory_points: - if(tpp.planner_plugin_name == "UnobstructedLaneChangePlugin" or tpp.planner_plugin_name == "CooperativeLaneChangePlugin"): - start_lane_change_x = tpp.x - start_lane_change_y = tpp.y - has_lane_change = True - break - - # First trajectory plan with a lane change has been found, can exit loop - if(has_lane_change): - break - - # If a trajectory plan with a lane change has been found, find the distance between the start of the lane change and the end of the geofence - if(has_lane_change): - distance_from_geofence_end_meters = ((start_lane_change_x - exit_geofence_x)**2 + (start_lane_change_y - exit_geofence_y)**2) ** 0.5 - distance_from_geofence_end_feet = distance_from_geofence_end_meters * 3.28 # Conversion from meters to feet - - # If the distance between the geofence exit and the start of the lane change meets the metric criteria, set the 'is_successful' flag - if(max_distance_from_geofence_end >= distance_from_geofence_end_feet >= min_distance_from_geofence_end): - is_successful = True - - # Print success/failure statement and return success flag - if (is_successful): - print("B-20 succeeded; lane change began " + str(distance_from_geofence_end_feet) + " feet after exiting the geofence") - else: - if (has_lane_change): - print("B-20 failed; lane change began " + str(distance_from_geofence_end_feet) + " feet after exiting the geofence") - else: - print("B-20 failed; no lane change was found after exiting the geofence.") - - return is_successful - -########################################################################################################### -# TIM B-21: The actual trajectory will start calling for acceleration back to the original speed limit -# no more than 30 feet away from the end of the geo-fenced area. -# RWM B-21: The actual trajectory will start calling for acceleration back to the original speed limit -# no more than 30 feet away from the end of the geo-fenced area. -########################################################################################################### -def check_acceleration_distance_after_geofence(bag, time_exit_geofence): - max_distance_from_geofence_end = 30.0 # (feet) Max distance from end of geofence for first lane change point - min_distance_from_geofence_end = 0.0 # (feet) Minimum distance from end of geofence for first lane change point - required_sequential_speed_increases = 5 # The number of sequential speed increases required to be considered the start of acceleration - conversion_meters_to_feet = 3.28084 # 1 meter is 3.28084 feet - - # Get the downtrack at the end of the geofence - downtrack_exit_geofence = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence): - downtrack_exit_geofence = msg.down_track - break - - # Variables to track during metric evaluation - has_found_start_of_accel = False - prev_speed = 0.0 - prev_t = rospy.Time() - has_found_start_of_accel = False - time_start_of_accel = rospy.Time() - is_first_speed = True - count_speed_increases = 0 - - # Get the timestamp associated with the start of vehicle acceleration after exiting the geofence - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_exit_geofence): - if is_first_speed: - prev_speed = msg.twist.linear.x - prev_t = t - is_first_speed = False - continue - - # Get start time of acceleration - current_speed = msg.twist.linear.x - duration_since_previous_speed = (t - prev_t).to_sec() - current_accel = (current_speed - prev_speed) / duration_since_previous_speed - #print("Current acceleration: " + str(current_accel) + " m/s^2; speed is " + str(current_speed) + " m/s") - - # Check if this is the start of acceleration - if count_speed_increases == 0: - time_start_of_accel = t # Always track the start time of consecutive speed increases - - if current_speed > prev_speed: - count_speed_increases += 1 - else: - count_speed_increases = 0 - - # End loop if reached the required number of consecutive speed increases - if count_speed_increases == required_sequential_speed_increases: - has_found_start_of_accel = True - break - - # Update variables - prev_t = t - prev_speed = current_speed - - # Get the distance after the geofence that the vehicle begins accelerating if any acceleration was found - if has_found_start_of_accel: - # Get the downtrack at the start of acceleration back to the original speed limit - downtrack_start_of_accel = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_of_accel): - downtrack_start_of_accel = msg.down_track - break - - # Obtain the distance (feet) after the geofence that the vehicle begins accelerating - distance_start_accel_after_geofence = (downtrack_start_of_accel - downtrack_exit_geofence) * conversion_meters_to_feet - - # Set is_successful flag and print debug statements - is_successful = False - if has_found_start_of_accel: - if distance_start_accel_after_geofence >= min_distance_from_geofence_end and distance_start_accel_after_geofence <= max_distance_from_geofence_end: - print("B-21 succeeded; vehicle acceleration began " + str(distance_start_accel_after_geofence) + " feet after exiting the geofence.") - is_successful = True - else: - print("B-21 failed; vehicle acceleration began " + str(distance_start_accel_after_geofence) + " feet after exiting the geofence.") - is_successful = False - else: - print("B-21 failed; no vehicle acceleration occurred after exiting the geofence.") - is_successful = False - - return is_successful - - -########################################################################################################### -# TIM B-22: The actual trajectory back to normal operations will include an acceleration portion and -# the average acceleration over the entire acceleration time shall be no less than 1 m/s^2. -# RWM B-22: The actual trajectory back to normal operations will include an acceleration portion and -# the average acceleration over the entire acceleration time shall be no less than 1 m/s^2. -########################################################################################################### -def check_acceleration_rate_after_geofence(bag, time_exit_geofence, original_speed_limit, advisory_speed_limit): - # Parameters used for metric evaluation - min_average_acceleration = 1.0 # m/s^2 - end_accel_percent_of_original_speed_limit = 0.90 - start_accel_percent_of_advisory_speed_limit = 1.10 - - # Variables to track during metric evaluation - speed_start_accel = 0.0 - speed_end_accel = 0.0 - time_start_accel = rospy.Time() - time_end_accel = rospy.Time() - has_found_start_of_accel = False - has_found_end_of_accel = False - - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_exit_geofence): - if has_found_start_of_accel: - accel = (msg.twist.linear.x - prev_speed) / (t-prev_time).to_sec() - #print("Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x) + "; " + str(accel) + " m/s^2") - prev_speed = msg.twist.linear.x - prev_time = t - - # Get start time of deceleration - if not has_found_start_of_accel and (msg.twist.linear.x > (start_accel_percent_of_advisory_speed_limit * advisory_speed_limit)): - print("Accel Start Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_start_accel = t - speed_start_accel = msg.twist.linear.x - has_found_start_of_accel = True - prev_speed = msg.twist.linear.x - prev_time = t - - # Get end time of deceleration - if has_found_start_of_accel and (msg.twist.linear.x > (end_accel_percent_of_original_speed_limit * original_speed_limit)): - print("Accel End Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_end_accel = t - speed_end_accel = msg.twist.linear.x - has_found_end_of_accel = True - break - - # If the full acceleration phase has been found, determine the average acceleration for this phase - if has_found_start_of_accel and has_found_end_of_accel: - average_acceleration = (speed_end_accel - speed_start_accel) / (time_end_accel - time_start_accel).to_sec() - print("Avg acceleration: " + str(average_acceleration)) - - # Print success/failure statement and return success flag - is_successful = False - if (has_found_start_of_accel and has_found_end_of_accel): - if average_acceleration >= min_average_acceleration: - print("B-15 succeeded; average acceleration after geofence was above 1.0 m/s^2: " + str(average_acceleration) + " m/s^2") - is_successful = True - else: - print("B-15 failed; average acceleration after geofence was below 1.0 m/s^2: " + str(average_acceleration) + " m/s^2") - is_successful = False - elif has_found_start_of_accel: - print("B-15 failed; no end of acceleration after exiting the geofence was found.") - is_successful = True - else: - print("B-15 failed; no acceleration after exiting the geofence was found.") - is_successful = False - - return is_successful - -########################################################################################################### -# TIM B-23: The planned route must end with the CP vehicle having been at steady state, after all other -# maneuvers, for at least 10 seconds. -# RWM B-23: The planned route must end with the CP vehicle having been at steady state, after all other -# maneuvers, for at least 10 seconds. -########################################################################################################### -def check_steady_state_after_geofence(bag, time_exit_geofence, time_end_engagement, original_speed_limit): - # Parameters used for metric evaluation - # (m/s) Threshold offset from speed limit; vehicle considered at steady state when its speed is within this offset of the speed limit - threshold_speed_limit_offset = 0.894 # 0.894 m/s is 2 mph - # (m/s) Minimum speed to be considered at steady state - min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset - # (m/s) Maximum speed to be considered at steady state - max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset - # (seconds) Minimum required threshold at steady state after completing all geofence-triggered maneuvers - min_steady_state_time = 10.0 - - # Conduct steady state evaluation - # Get the start time of the vehicle reaching steady state (if one exists) - has_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_exit_geofence, end_time = time_end_engagement): - # Vehicle has reached steady state when its speed within threshold range of steady state speed - if (max_steady_state_speed >= msg.twist.linear.x >= min_steady_state_speed): - time_start_steady_state = t - has_steady_state = True - break - - # If the start time of steady state was found, find the longest duration of steady state time - has_passed_steady_state_time_threshold = False - max_steady_state_duration = 0.0 - if (has_steady_state): - has_passed_steady_state_time_threshold = False - is_at_steady_state = True - steady_state_duration = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_steady_state, end_time = time_end_engagement): - current_speed = msg.twist.linear.x - - # If system is entering steady state, reset the steady state start time: - if ((max_steady_state_speed >= current_speed >= min_steady_state_speed) and not is_at_steady_state): - is_at_steady_state = True - time_start_steady_state = t - - # If system is maintaining steady state, check if it has passed the threshold time of continuous steady state: - elif ((max_steady_state_speed >= current_speed >= min_steady_state_speed) and is_at_steady_state): - steady_state_duration = (t - time_start_steady_state).to_sec() - - if steady_state_duration > max_steady_state_duration: - max_steady_state_duration = steady_state_duration - - if (steady_state_duration >= min_steady_state_time): - has_passed_steady_state_time_threshold = True - - # If system has exited steady state, reset the steady state flag - elif ((max_steady_state_speed <= current_speed or current_speed <= min_steady_state_speed) and is_at_steady_state): - is_at_steady_state = False - - if (has_passed_steady_state_time_threshold): - print("B-23 succeeded; system reached continuous steady state for " + str(max_steady_state_duration) + " seconds after geofence-triggered maneuvers.") - is_successful = True - else: - if has_steady_state: - print("B-23 failed; system reached continuous steady state for " + str(steady_state_duration) + " seconds after geofence-triggered maneuvers. " \ - + " At least " + str(min_steady_state_time) + " seconds required.") - is_successful = False - if not has_steady_state: - print("B-23 failed; system did not reach steady state after geofence-triggered maneuvers.") - is_successful = False - - return is_successful - -########################################################################################################### -# TIM B-24: The entire scenario will satisfy all previous criteria using any of the speeds given here for -# the "regular speed limit" (i.e. the speed limit when not in the geo-fence). (25 mph) -# RWM B-24: The entire scenario will satisfy all previous criteria using any of the speeds given here for -# the "regular speed limit" (i.e. the speed limit when not in the geo-fence). (25 mph) -########################################################################################################### -def check_speed_limit_when_not_in_geofence(bag, time_start_engagement, time_enter_geofence, time_exit_geofence, time_end_engagement, original_speed_limit): - # (m/s) Threshold offset from speed limit to account for floating point precision - threshold_speed_limit_offset = 0.01 - max_speed_limit = original_speed_limit + threshold_speed_limit_offset # (m/s) - min_speed_limit = original_speed_limit - threshold_speed_limit_offset # (m/s) - - # Tolerance for the time between the geofence start and end and finding a speed limit that belongs in the geofence: - # Note: This tolerance is due to timing not being fully-synchronized between /environment/active_geofence and all other topics - time_tolerance_geofence = 0.80 # seconds - - # Check speed limit before entering geofence - is_correct_speed_limit_before_geofence = False - time_incorrect_speed_limit_before_geofence = 0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_enter_geofence): - # The first correct speed limit has been found - if ((max_speed_limit >= msg.speed_limit >= min_speed_limit) and not is_correct_speed_limit_before_geofence): - is_correct_speed_limit_before_geofence = True - # An incorrect speed limit has been found; will trigger a failure if not within tolerance of the geofence entrance - elif((msg.speed_limit >= max_speed_limit or msg.speed_limit <= min_speed_limit) and is_correct_speed_limit_before_geofence): - time_incorrect_speed_limit_before_geofence = abs(time_enter_geofence - t).to_sec() - - # Not a failure if within tolerance time of the geofence entrance - if (time_incorrect_speed_limit_before_geofence <= time_tolerance_geofence): - is_correct_speed_limit_before_geofence = True - continue - - # Failure if not within tolerance time of the geofence entrance - print("Speed limit " + str(msg.speed_limit) + " m/s found at " + str(t.to_sec()) + \ - ", which is " + str(time_incorrect_speed_limit_before_geofence) + " seconds before entering the geofence.") - is_correct_speed_limit_before_geofence = False - break - - # Check speed limit after exiting geofence - is_correct_speed_limit_after_geofence = False - time_incorrect_speed_limit_after_geofence = 0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence, end_time = time_end_engagement): - # Speed limit is correct for the first time after exiting the geofence - if ((max_speed_limit >= msg.speed_limit >= min_speed_limit) and not is_correct_speed_limit_after_geofence): - time_incorrect_speed_limit_after_geofence = abs(t - time_exit_geofence).to_sec() - if (time_incorrect_speed_limit_after_geofence <= time_tolerance_geofence): - is_correct_speed_limit_after_geofence = True - continue - - # Failure; speed limit is incorrect and not within tolerance time of exiting geofence: - elif((msg.speed_limit >= max_speed_limit or msg.speed_limit <= min_speed_limit) and not is_correct_speed_limit_after_geofence): - time_incorrect_speed_limit_after_geofence = abs(t - time_exit_geofence).to_sec() - print("Speed limit " + str(msg.speed_limit) + " m/s found at " + str(t.to_sec()) + \ - ", which is " + str(time_incorrect_speed_limit_after_geofence) + " seconds after exiting the geofence.") - is_correct_speed_limit_after_geofence = False - break - - # Failure; speed limit is incorrect after having been correct before - elif((msg.speed_limit >= max_speed_limit or msg.speed_limit <= min_speed_limit) and is_correct_speed_limit_after_geofence): - is_correct_speed_limit_after_geofence = False - break - - # Print success/failure statement and return success flag - if (not is_correct_speed_limit_before_geofence and not is_correct_speed_limit_after_geofence): - print("B-24 failed; speed limit was not always " + str(original_speed_limit) + " m/s before OR after the geofence.") - is_successful = False - elif(not is_correct_speed_limit_before_geofence): - print("B-24 failed; speed limit was not always " + str(original_speed_limit) + " m/s before the geofence.") - is_successful = False - elif(not is_correct_speed_limit_after_geofence): - print("B-24 failed; speed limit was not always " + str(original_speed_limit) + " m/s after the geofence.") - is_successful = False - else: - print("B-24 succeeded; speed limit was always " + str(original_speed_limit) + " m/s before AND after the geofence.") - is_successful = True - - return is_successful - -########################################################################################################### -# TIM B-25: The speed limit given by the CM vehicle shall be 10 mph less -# than the regular speed limit. -# RWM B-25: The speed limit given by CARMA Cloud shall be 10 mph less -# than the regular speed limit. -########################################################################################################### -def check_advisory_speed_limit(bag, time_start_engagement, advisory_speed_limit, original_speed_limit): - # (m/s) Required offset from normal speed limit required for the advisory speed limit - speed_limit_offset = 4.4704 # 4.4704 m/s is 10 mph - # (m/s) Threshold offset from expected advisory speed limit to account for floating point precision - threshold_speed_limit_offset = 0.01 - max_advisory_speed_limit = (original_speed_limit - speed_limit_offset) + threshold_speed_limit_offset - min_advisory_speed_limit = (original_speed_limit - speed_limit_offset) - threshold_speed_limit_offset - - # Evaluate speed limits, print success/failure statement, and return success flag - advisory_speed_limit_mph = int(advisory_speed_limit * 2.23694) # Conversion of m/s to mph - original_speed_limit_mph = int(original_speed_limit * 2.23694) # Conversion of m/s to mph - speed_limit_offset_mph = int(speed_limit_offset * 2.23694) # Conversion of m/s to mph - if (max_advisory_speed_limit >= advisory_speed_limit >= min_advisory_speed_limit): - print("B-25 succeeded; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ - + " mph), which is " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ - + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") - is_successful = True - else: - print("B-25 failed; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ - + " mph), which is not " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ - + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") - is_successful = False - - return is_successful - -# Main Function; run all tests from here -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - source_folder = sys.argv[1] - - # Re-direct the output of print() to a specified .txt file: - orig_stdout = sys.stdout - current_time = datetime.datetime.now() - text_log_filename = "Results_" + str(current_time) + ".txt" - text_log_file_writer = open(text_log_filename, 'w') - sys.stdout = text_log_file_writer - - # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - csv_results_filename = "Results_" + str(current_time) + ".csv" - csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", - "B-1 Result", "B-2 Result", "B-3 Result", "B-4 Result", "B-5 Result", "B-6 Result", - "B-7 Result", "B-8 Result", "B-9 Result", "B-10 Result","B-11 Result", "B-12 Result", - "B-13 Result", "B-14 Result", "B-15 Result", "B-16 Result", "B-17 Result", "B-18 Result", - "B-19 Result", "B-20 Result", "B-21 Result", "B-22 Result", "B-23 Result", "B-24 Result", - "B-25 Result", "B-26 Result", "B-27 Result", "B-28 Result"]) - - # Create list of TIM Black Pacifica bag files to be processed - TIM_black_pacifica_bag_files = [] - - # Create list of TIM Ford Fusion bag files to be processed - TIM_ford_fusion_bag_files = ["_2021-06-24-18-03-35_down-selected.bag", - "_2021-06-24-18-14-20_down-selected.bag", - "_2021-06-24-18-20-00_down-selected.bag", - "_2021-06-24-18-26-11_down-selected.bag", - "_2021-06-24-18-31-59_down-selected.bag"] - - # Create list of TIM Blue Lexus bag files to be processed - TIM_blue_lexus_bag_files = [] - - # Concatenate all TIM bag files into one list - TIM_bag_files = TIM_black_pacifica_bag_files + TIM_ford_fusion_bag_files + TIM_blue_lexus_bag_files - - # Create list of RWM Black Pacifica bag files to be processed - RWM_black_pacifica_bag_files = ["_2021-06-23-13-10-28_down-selected.bag", - "_2021-06-23-13-20-05_down-selected.bag", - "_2021-06-23-13-42-56_down-selected.bag", - "_2021-06-23-13-52-29_down-selected.bag", - "_2021-06-23-14-02-22_down-selected.bag", - "_2021-06-23-14-17-32_down-selected.bag"] - - # Create list of RWM Ford Fusion bag files to be processed - RWM_ford_fusion_bag_files = ["_2021-06-22-19-22-48_down-selected.bag", - "_2021-06-22-19-29-24_down-selected.bag", - "_2021-06-22-19-38-00_down-selected.bag", - "_2021-06-22-19-45-31_down-selected.bag", - "_2021-06-22-19-53-39_down-selected.bag", - "_2021-06-24-17-04-56_down-selected.bag"] - - # Create list of RWM Blue Lexus bag files to be processed - RWM_blue_lexus_bag_files = [] - - # Concatenate all RWM bag files into one list - RWM_bag_files = RWM_black_pacifica_bag_files + RWM_ford_fusion_bag_files + RWM_blue_lexus_bag_files - - TIM_and_RWM_bag_files = TIM_bag_files + RWM_bag_files - - # Loop to conduct data anlaysis on each bag file: - for bag_file in TIM_and_RWM_bag_files: - print("*****************************************************************") - print("Processing new bag: " + str(bag_file)) - if bag_file in TIM_bag_files: - print("TIM Test Case") - elif bag_file in RWM_bag_files: - print("RWM Test Case") - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(TIM_and_RWM_bag_files.index(bag_file) + 1) + " of " + str(len(TIM_and_RWM_bag_files)) + ")") - sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - bag = rosbag.Bag(bag_file_path) - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - # Get the rosbag times associated with entering and exiting the active geofence - print("Getting geofence times at " + str(datetime.datetime.now())) - time_enter_geofence, time_exit_geofence, found_geofence_times = get_geofence_entrance_and_exit_times(bag) - print("Got geofence times at " + str(datetime.datetime.now())) - if (not found_geofence_times): - print("Could not find geofence entrance and exit times in bag file.") - continue - - # Get the rosbag times associated with the starting engagement and ending engagement for the TIM use case test (15-25 seconds) - print("Getting engagement times at " + str(datetime.datetime.now())) - time_test_start_engagement, time_test_end_engagement, found_test_times = get_test_case_engagement_times(bag, time_enter_geofence, time_exit_geofence) - print("Got engagement times at " + str(datetime.datetime.now())) - if (not found_test_times): - print("Could not find test case engagement start and end times in bag file.") - continue - - # Debug Statements - print("Engagement starts at " + str(time_test_start_engagement)) - print("Entered Geofence at " + str(time_enter_geofence)) - print("Exited Geofence at " + str(time_exit_geofence)) - print("Engagement ends at " + str(time_test_end_engagement)) - print("Time spent in geofence: " + str((time_exit_geofence - time_enter_geofence).to_sec()) + " seconds") - print("Time spent engaged: " + str((time_test_end_engagement - time_test_start_engagement).to_sec()) + " seconds") - - # Pre-processed data for data analysis: - closed_lanelets = [24078] - print("Assuming Closed Lanelets: " + str(closed_lanelets)) - - original_speed_limit = get_route_original_speed_limit(bag, time_test_start_engagement) # Units: m/s - print("Original Speed Limit is " + str(original_speed_limit) + " m/s") - - # Update the exit geofence time to be based off of /guidance/route_state for improved accuracy - time_exit_geofence = adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit) - print("Adjusted geofence exit time (based on /guidance/route_state): " + str(time_exit_geofence)) - - # Initialize results - b_1_result = None - b_2_result = None - b_4_result = None - b_10_result = None - b_11_result = None - b_13_result = None - b_15_result = None - b_20_result = None - b_21_result = None - b_22_result = None - b_23_result = None - b_24_result = None - b_25_result = None - b_28_result = None - - # Metrics B-28 - if bag_file in TIM_bag_files: - minimum_gap, advisory_speed_limit, event_type, time_received_first_msg, b_28_result = get_TIM_mobility_operation_data(bag) - elif bag_file in RWM_bag_files: - minimum_gap, advisory_speed_limit, time_received_first_msg, b_28_result = get_RWM_TCM_data(bag) - - # Convert advisory speed limit from B-28 to m/s for future metric evaluations - advisory_speed_limit = advisory_speed_limit * 0.44704 # Conversion from mph to m/s - - # Metrics B-1 and B-11 - b_1_result, b_11_result = check_geofence_route_metrics(bag, closed_lanelets) - - # Metrics B-10 - b_10_result = check_in_geofence_speed_limits(bag, time_test_start_engagement, time_exit_geofence, advisory_speed_limit) - - # Metrics B-2 - b_2_result = check_steady_state_before_first_received_message(bag, time_test_start_engagement, time_received_first_msg, original_speed_limit) - - # Metrics B-4 (Currently B-4 is evaluated using the speed plot from Volpe's generate_plots.py script) - b_4_result = None - - # Metrics B-13 - b_13_result = check_lane_merge_before_geofence(bag, time_test_start_engagement, time_enter_geofence) - - # Metrics B-15 - b_15_result = check_deceleration_before_geofence(bag, time_enter_geofence, original_speed_limit, advisory_speed_limit) - - # Metrics B-20 - b_20_result = check_lane_change_after_geofence(bag, time_exit_geofence) - - # Metrics B-21 - b_21_result = check_acceleration_distance_after_geofence(bag, time_exit_geofence) - - # Metrics B-22 - b_22_result = check_acceleration_rate_after_geofence(bag, time_exit_geofence, original_speed_limit, advisory_speed_limit) - - # Metrics B-23 - b_23_result = check_steady_state_after_geofence(bag, time_exit_geofence, time_test_end_engagement, original_speed_limit) - - # Metrics B-24 - b_24_result = check_speed_limit_when_not_in_geofence(bag, time_test_start_engagement, time_enter_geofence, time_exit_geofence, time_test_end_engagement, original_speed_limit) - - # Metrics B-25 - b_25_result = check_advisory_speed_limit(bag, time_test_end_engagement, advisory_speed_limit, original_speed_limit) - - - # Get vehicle type that this bag file is from - vehicle_name = "Unknown" - if bag_file in TIM_black_pacifica_bag_files or RWM_black_pacifica_bag_files: - vehicle_name = "Black Pacifica" - elif bag_file in TIM_ford_fusion_bag_files or RWM_ford_fusion_bag_files: - vehicle_name = "Ford Fusion" - elif bag_file in TIM_blue_lexus_bag_files or RWM_blue_lexus_bag_files: - vehicle_name = "Blue Lexus" - else: - vehicle_name = "N/A" - - # Get test type that this bag file is for - test_type = "Unknown" - if bag_file in TIM_bag_files: - test_type = "TIM" - elif bag_file in RWM_bag_files: - test_type = "RWM" - - # Write simple pass/fail results to .csv file for appropriate row: - csv_results_writer.writerow([bag_file, vehicle_name, test_type, - b_1_result, b_2_result, "B-3", b_4_result, "B-5", "B-6", "B-7", "B-8", "B-9", b_10_result, - b_11_result, "B-12", b_13_result, "B-14", b_15_result, "B-16", "B-17", "B-18", "B-19", b_20_result, - b_21_result, b_22_result, b_23_result, b_24_result, b_25_result, "B-26", "B-27", b_28_result]) - - sys.stdout = orig_stdout - text_log_file_writer.close() - return - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_basic_travel_rosbags.py b/engineering_tools/data_processing/analyze_basic_travel_rosbags.py deleted file mode 100644 index be5bbae37f..0000000000 --- a/engineering_tools/data_processing/analyze_basic_travel_rosbags.py +++ /dev/null @@ -1,1943 +0,0 @@ -#!/usr/bin/python3 - -# Copyright (C) 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. - -from inspect import TPFLAGS_IS_ABSTRACT -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime - -# HOW TO USE SCRIPT: -# Run the following in a terminal to download dependencies: -# sudo add-apt-repository ppa:deadsnakes/ppa -# sudo apt-get update -# sudo apt install python3.7 -# python3.7 -m pip install --upgrade pip -# python3.7 -m pip install matplotlib -# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg -# python3.7 -m pip install lz4 -# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ -# In terminal, navigate to the directory that contains this python script and run the following: -# python3.7 analyze_basic_travel_rosbags.py - -# Helper Function: Get times associated with the system entering the geofence and exiting the geofence -def get_geofence_entrance_and_exit_times(bag): - # Initialize geofence entrance and exit times - time_enter_active_geofence = rospy.Time() # Time that the vehicle has entered the geofence - time_exit_active_geofence = rospy.Time() # Time that the vehicle has exited the geofence-------------------------------------------------------------------------------------------------------- - - # Find geofence entrance and exit times - is_on_active_geofence = False - found_geofence_entrance_time = False - found_geofence_exit_time = False - for topic, msg, t in bag.read_messages(topics=['/environment/active_geofence']): - # If first occurrence of being in the active geofence, set the start time - if (msg.is_on_active_geofence and not is_on_active_geofence): - time_enter_active_geofence = t - #print("Entered geofence at " + str(t.to_sec())) - found_geofence_entrance_time = True - is_on_active_geofence = True - - # If final occurrence of being in the active geofence, set the end time - if (not msg.is_on_active_geofence and is_on_active_geofence): - time_exit_active_geofence = t - found_geofence_exit_time = True - break - - # Check if both geofence start and end time were found - found_geofence_times = False - if (found_geofence_entrance_time and found_geofence_exit_time): - found_geofence_times = True - - return time_enter_active_geofence, time_exit_active_geofence, found_geofence_times - -# Helper Function: Get the original speed limit for the lanelets within the vehicle's route -# Note: Assumes that all lanelets in the route share the same speed limit prior to the first geofence CARMA Cloud message being processed. -def get_route_original_speed_limit(bag, time_test_start_engagement): - # Initialize the return variable - original_speed_limit = 0.0 - - # Find the speed limit associated with the first lanelet when the system first becomes engaged - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_test_start_engagement): - original_speed_limit = msg.speed_limit - break - - return original_speed_limit - -# Helper function: Begin with the time that the vehicle exits the active geofence according to -# /guidance/active_geofence topic. This helper function adjusts the time to be -# based on /guidance/route_state in order to be more accurate. -def adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit): - # Get the true time of the end of the geofence, based on when /guidance/route_state displays the - # original speed limit for the current vehicle location - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence): - if msg.speed_limit == original_speed_limit: - true_time_exit_geofence = t - break - - return true_time_exit_geofence - -# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section -def get_test_case_engagement_times(bag, time_enter_active_geofence, time_exit_active_geofence): - # Initialize system engagement start and end times - time_start_engagement = rospy.Time() - time_stop_engagement = rospy.Time() - - # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section - is_engaged = False - found_engagement_times = False - has_reached_geofence_entrance = False - has_reached_geofence_exit = False - for topic, msg, t in bag.read_messages(topics=['/guidance/state']): - # If entering engagement, track this start time - if (msg.state == 4 and not is_engaged): - time_start_engagement = t - is_engaged = True - - # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance - # state can be published. - if (msg.state == 4): - time_last_engaged = t - - # If exiting engagement, check whether this period of engagement included the geofence entrance and exit times - elif (msg.state != 4 and is_engaged): - is_engaged = False - time_stop_engagement = t - - # Check if the engagement start time was before the geofence entrance and exit times - if (time_start_engagement <= time_enter_active_geofence and t >= time_enter_active_geofence): - has_reached_geofence_entrance = True - if (time_start_engagement <= time_exit_active_geofence and t >= time_exit_active_geofence): - has_reached_geofence_exit = True - - # Set flag if this engagement period includes both the geofence entrance and exit times - if (has_reached_geofence_entrance and has_reached_geofence_exit): - found_test_case_engagement_times = True - break - - # If CARMA ended engagement before guidance state could be updated, check if the last recorded - # time of engagement came after exiting the geofence - if not found_engagement_times: - if time_last_engaged >= time_exit_active_geofence: - time_stop_engagement = time_last_engaged - found_engagement_times = True - - return time_start_engagement, time_stop_engagement, found_engagement_times - -# Helper Function: Get the lanelet IDs that are included in the geofence. -def get_geofence_lanelets(bag, time_start_engagement, advisory_speed_limit): - # Loop through route_state topic and add all unique - lanelets_in_geofence = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): - if (msg.speed_limit - advisory_speed_limit < 0.01): - if msg.lanelet_id not in lanelets_in_geofence: - lanelets_in_geofence.append(msg.lanelet_id) - - print("Lanelet IDs in the geofence: " + str(lanelets_in_geofence)) - - return lanelets_in_geofence - -# Helper Function: Get the route downtrack associated with the geofence entrance -def get_geofence_entrance_downtrack(bag, time_enter_geofence): - - # Return the first 'downtrack' published to /guidance/route_state after entering the geofence - downtrack_enter_geofence = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_enter_geofence): - downtrack_enter_geofence = msg.down_track - break - - print("Downtrack of geofence entrance: " + str(downtrack_enter_geofence) + " meters") - - return downtrack_enter_geofence - -# Helper Function: Print out the times associated with the vehicle entering each new lanelet according to /guidance/route_state -def print_lanelet_entrance_times(bag, time_start_engagement): - # Print out time vehicle enters each lanelet according to /guidance/route_state - id = 0 - print("/guidance/route_state lanelet change times:") - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): - if msg.lanelet_id != id: - print("Time: " + str(t.to_sec()) + "; Lanelet: " + str(msg.lanelet_id) + "; Speed Limit: " + str(msg.speed_limit)) - id = msg.lanelet_id - - return - -# Helper Function: Get all important timestamps for platoon negotiation from the Rear Follower Vehicle's perspective -# Message 1: Follower Broadcasts "JOIN_PLATOON_AT_REAR" MobilityRequest -# Message 2: Follower Receives MobilityRequest from Leader with is_accepted==True -# Message 3: Follower Broadcasts "PLATOON_FOLLOWER_JOIN" MobilityRequest -# Message 4: Follower Receives MobilityRequest from Leader with is_accepted==True -def get_follower_platoon_negotiation_times(bag, time_test_start_engagement): - # Obtain times that vehicle receives an 'is_accepted' mobility_request message - # Note: First acceptance is in response to Follower vehicle's 'JOIN_PLATOON_AT_REAR' MobilityRequest - # Second acceptance is in response to Follower vehicle's 'PLATOON_FOLLOWER_JOIN' MobilityRequest (End of Platoon Negotiation) - time_received_first_acceptance = rospy.Time() - time_received_second_acceptance = rospy.Time() - has_received_first_acceptance = False - has_received_second_acceptance = False - has_received_additional_acceptances = False - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response'], start_time = time_test_start_engagement): - if msg.is_accepted == True: - if not has_received_first_acceptance: - print("Received first MobilityRequest with is_accepted==True at time " + str(t)) - time_received_first_acceptance = t - has_received_first_acceptance = True - elif not has_received_second_acceptance: - print("Received second MobilityRequest with is_accepted==True at time " + str(t)) - time_received_second_acceptance = t - has_received_second_acceptance = True - else: - print("Error: Received additional MobilityRequest with is_accepted==True at time " + str(t)) - has_received_additional_acceptances = True - - # Obtain times that vehicle broadcasts its final 'JOIN_PLATOON_AT_REAR' and 'PLATOON_FOLLOWER_JOIN' MobilityRequest messages - # Note: 'JOIN_PLATOON_AT_REAR' is request from the Follower vehicle to join the platoon at its rear (Beginning of Platoon Negotiation) - # 'PLATOON_FOLLOWER_JOIN' is indication that the Follower vehicle is officially joining the platoon - has_sent_last_join_at_rear = False - has_sent_last_follower_join = False - has_sent_additional_join_at_rear = False - has_sent_additional_follower_join = False - time_last_sent_join_at_rear = rospy.Time() - time_last_sent_follower_join = rospy.Time() - previous_plan_type = -1 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request'], start_time = time_test_start_engagement): - # Track whether the previous MobilityRequest plan_type was the last 'JOIN_PLATOON_AT_REAR' message - if previous_plan_type == 3 and previous_plan_type != msg.plan_type.type: - if has_sent_last_join_at_rear: - print("Error: New 'last' sent 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_sent_join_at_rear)) - has_sent_additional_join_at_rear = True - else: - has_sent_last_join_at_rear = True - print("Sent last 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_sent_join_at_rear)) - # Track whether the previous MobilityRequest was the last 'PLATOON_FOLLOWER_JOIN' message - elif previous_plan_type == 4 and previous_plan_type != msg.plan_type.type: - if has_sent_last_follower_join: - print("Error: New 'last' sent 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_sent_join_at_rear)) - has_sent_additional_follower_join = True - else: - has_sent_last_follower_join = True - print("Sent last 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_sent_follower_join)) - - # Update 'previous' values based on the current timestamp and plan type - if msg.plan_type.type == 3: - time_last_sent_join_at_rear = t - previous_plan_type = msg.plan_type.type - elif msg.plan_type.type == 4: - time_last_sent_follower_join = t - previous_plan_type = msg.plan_type.type - - # Track whether the last broadcasted MobilityRequest message was a 'JOIN_PLATOON_AT_REAR' message - if previous_plan_type == 3: - if has_sent_last_join_at_rear: - print("Error: New 'last' sent 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_sent_join_at_rear)) - has_sent_additional_join_at_rear = True - else: - print("Sent last 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_sent_join_at_rear)) - has_sent_last_join_at_rear = True - # Track whether the last broadcasted MobilityRequest message was a 'PLATOON_FOLLOWER_JOIN' message - elif previous_plan_type == 4: - if has_sent_last_follower_join: - print("Error: New 'last' sent 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_sent_join_at_rear)) - has_sent_additional_follower_join = True - else: - print("Sent last 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_sent_follower_join)) - has_sent_last_follower_join = True - - if has_received_additional_acceptances or has_sent_additional_join_at_rear or has_sent_additional_follower_join: - print("WARNING: Platooning metrics may be incorrect; multiple occurrences of one or more negotiation messages has occurred.") - else: - print("Platooning negotiation was successful.") - - return time_last_sent_follower_join, time_last_sent_join_at_rear, time_received_first_acceptance, time_received_second_acceptance - -# Helper Function: Get all important timestamps for platoon negotiation from the Front Vehicle's perspective -# Message 1: Leader receives "JOIN_PLATOON_AT_REAR" MobilityRequest -# Message 2: Leader broadcasts MobilityRequest from Leader with is_accepted==True -# Message 3: Leader Broadcasts "PLATOON_FOLLOWER_JOIN" MobilityRequest -# Message 4: Follower Receives MobilityRequest from Leader with is_accepted==True -def get_leader_platoon_negotiation_times(bag, time_test_start_engagement): - # Obtain times that vehicle broadcasts an 'is_accepted' mobility_request message - # Note: First acceptance is in response to Follower vehicle's 'JOIN_PLATOON_AT_REAR' MobilityRequest - # Second acceptance is in response to Follower vehicle's 'PLATOON_FOLLOWER_JOIN' MobilityRequest (End of Platoon Negotiation) - time_sent_first_acceptance = rospy.Time() - time_sent_second_acceptance = rospy.Time() - has_sent_first_acceptance = False - has_sent_second_acceptance = False - has_sent_additional_acceptances = False - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_response'], start_time = time_test_start_engagement): - if msg.is_accepted == True: - if not has_sent_first_acceptance: - print("Sent first MobilityRequest with is_accepted==True at time " + str(t)) - time_sent_first_acceptance = t - has_sent_first_acceptance = True - elif not has_sent_second_acceptance: - print("Sent second MobilityRequest with is_accepted==True at time " + str(t)) - time_sent_second_acceptance = t - has_sent_second_acceptance = True - else: - print("Error: Sent additional MobilityRequest with is_accepted==True at time " + str(t)) - has_sent_additional_acceptances = True - - # Obtain times that vehicle broadcasts its final 'JOIN_PLATOON_AT_REAR' and 'PLATOON_FOLLOWER_JOIN' MobilityRequest messages - # Note: 'JOIN_PLATOON_AT_REAR' is request from the Follower vehicle to join the platoon at its rear (Beginning of Platoon Negotiation) - # 'PLATOON_FOLLOWER_JOIN' is indication that the Follower vehicle is officially joining the platoon - has_received_last_join_at_rear = False - has_received_last_follower_join = False - has_received_additional_join_at_rear = False - has_received_additional_follower_join = False - time_last_received_join_at_rear = rospy.Time() - time_last_received_follower_join = rospy.Time() - previous_plan_type = -1 - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_request'], start_time = time_test_start_engagement): - # Track whether the previous MobilityRequest plan_type was the last 'JOIN_PLATOON_AT_REAR' message - if previous_plan_type == 3 and previous_plan_type != msg.plan_type.type: - if has_received_last_join_at_rear: - print("Error: New 'last' received 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_received_join_at_rear)) - has_received_additional_join_at_rear = True - else: - has_received_last_join_at_rear = True - print("Received last 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_received_join_at_rear)) - # Track whether the previous MobilityRequest was the last 'PLATOON_FOLLOWER_JOIN' message - elif previous_plan_type == 4 and previous_plan_type != msg.plan_type.type: - if has_received_last_follower_join: - print("Error: New 'last' received 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_received_join_at_rear)) - has_received_additional_follower_join = True - else: - has_received_last_follower_join = True - print("Received last 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_received_follower_join)) - - # Update 'previous' values based on the current timestamp and plan type - if msg.plan_type.type == 3: - time_last_received_join_at_rear = t - previous_plan_type = msg.plan_type.type - elif msg.plan_type.type == 4: - time_last_received_follower_join = t - previous_plan_type = msg.plan_type.type - - # Track whether the last broadcasted MobilityRequest message was a 'JOIN_PLATOON_AT_REAR' message - if previous_plan_type == 3: - if has_received_last_join_at_rear: - print("Error: New 'last' received 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_received_join_at_rear)) - has_received_additional_join_at_rear = True - else: - has_received_last_join_at_rear = True - print("Received last 'JOIN_PLATOON_AT_REAR' at time " + str(time_last_received_join_at_rear)) - # Track whether the last broadcasted MobilityRequest message was a 'PLATOON_FOLLOWER_JOIN' message - elif previous_plan_type == 4: - if has_received_last_follower_join: - print("Error: New 'last' received 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_received_join_at_rear)) - has_received_additional_follower_join = True - else: - has_received_last_follower_join = True - print("Received last 'PLATOON_FOLLOWER_JOIN' at time " + str(time_last_received_follower_join)) - - if has_sent_additional_acceptances or has_received_additional_join_at_rear or has_received_additional_follower_join: - print("WARNING: Platooning metrics may be incorrect; multiple occurrences of one or more negotiation messages has occurred.") - else: - print("Platooning negotiation was successful.") - - return time_last_received_follower_join, time_last_received_join_at_rear, time_sent_first_acceptance, time_sent_second_acceptance - -# Helper Function: Obtain the total amount of time spent conducting platoon negotiation from the Rear Vehicle's perspective -# Note: Platoon Negotiation is time between the last broadcasted "JOIN_PLATOON_AT_REAR" message and -# the second received MobilityRequest from Leader with is_accepted==True -def get_platoon_negotiation_duration(time_end_negotiation, time_start_negotiation, is_leader = False): - duration_platoon_negotiation = (time_end_negotiation - time_start_negotiation).to_sec() - - if is_leader: - print("Time required for platoon negotiation: " + str(duration_platoon_negotiation) + " sec (LEADER)") - else: - print("Time required for platoon negotiation: " + str(duration_platoon_negotiation) + " sec (FOLLOWER)") - - return duration_platoon_negotiation - -########################################################################################################### -# Basic Travel B-19: The information communicated by the CARMA Cloud vehicle is the advisory speed limit -# in the geofenced area. -########################################################################################################### -def get_basic_travel_TCM_data(bag): - # Check that TCM Message is received for max speed (an advisory speed limit) - has_advisory_speed = False - time_first_msg_received = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - if msg.tcmV01.params.detail.choice == 12: - time_first_msg_received = t - advisory_speed = msg.tcmV01.params.detail.maxspeed - has_advisory_speed = True - - print("TCM Messages Received: Advisory Speed: " + str(advisory_speed) + " mph") - - break - - is_successful = False - if has_advisory_speed: - print("B-19 succeeded; TCM message received with advisory speed limit " + str(advisory_speed) + " mph") - is_successful = True - else: - print("B-19 failed; no TCM message with an advisory speed limit was received.") - is_successful = True - - return advisory_speed, time_first_msg_received, is_successful - -########################################################################################################### -# BASIC TRAVEL B-1: Geofenced area is a part of the initial route plan. -########################################################################################################### -def check_basic_travel_geofence_lanelets_in_initial_route(bag, lanelets_in_geofence): - # Get each set route from the bag file (includes set routes and updated/re-rerouted routes) - shortest_path_lanelets = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route']): - # Print Debug Statement that includes the list of lanelet IDs in the route: - print("Shortest Path Route Update at " + str(t.to_sec()) + ": " + str(msg.shortest_path_lanelet_ids)) - - shortest_path_lanelets.append([]) - for lanelet_id in msg.shortest_path_lanelet_ids: - shortest_path_lanelets[-1].append(lanelet_id) - - # Check whether the geofence lanelets are included in the initial route - initial_route_includes_geofence_lanelets = True # Flag for BT-B-1 success - initial_route_lanelets = shortest_path_lanelets[0] - for geofence_lanelet_id in lanelets_in_geofence: - if geofence_lanelet_id not in initial_route_lanelets: - print("Geofence Lanelet ID " + str(geofence_lanelet_id) + " is not in initial route.") - initial_route_includes_geofence_lanelets = False - break - - # Print result statements and return success flags - if (initial_route_includes_geofence_lanelets): - print("B-1 succeeded; all geofence lanelets " + str(lanelets_in_geofence) + " were in the initial route") - else: - print("B-1 failed: not all geofence lanelets " + str(lanelets_in_geofence) + " were in the initial route.") - - return initial_route_includes_geofence_lanelets - -########################################################################################################### -# BASIC TRAVEL B-2: The vehicle receives a message from CARMA Cloud informing of an advisory speed limit -# ahead. The vehicle processes this information. -########################################################################################################### -def check_geofence_lanelet_speed_limits(bag, lanelets_in_geofence, advisory_speed_limit): - epsilon = 0.01 - has_correct_geofence_lanelet_speed_limits = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): - - # Failure if the current lanelet is one of the geofence lanelets and its speed limit doesn't match the advisory speed limit - if msg.lanelet_id in lanelets_in_geofence: - if (abs(msg.speed_limit - advisory_speed_limit) >= epsilon): - print("Lanelet ID " + str(msg.lanelet_id) + " has speed limit of " + str(msg.speed_limit) + " m/s. " + \ - "Does not match advisory speed limit of " + str(advisory_speed_limit) + " m/s.") - has_correct_geofence_lanelet_speed_limits = False - break - - if has_correct_geofence_lanelet_speed_limits: - print("B-2 succeeded; all geofence lanelets " + str(lanelets_in_geofence) + " had advisory speed limit of " + \ - str(advisory_speed_limit) + " m/s") - else: - print("B-2 failed; not all geofence lanelets had an advisory speed limit of " + str(advisory_speed_limit) + " m/s") - - return has_correct_geofence_lanelet_speed_limits - -########################################################################################################### -# Basic Travel B-3: The vehicle receives the message from CARMA Cloud, which informs of the speed limit -# ahead, early enough that the vehicle could decelerate at a rate less than 2 m/s^2 -# in order to enter the geofence at the advisory speed limit. -########################################################################################################### -def check_time_received_first_TCM_message(bag, time_received_first_msg, time_enter_geofence, advisory_speed_limit): - # (m/s^2) Maximum deceleration limit allowed to slow down for geofence's advisory speed limit - max_decel_limit = 2.0 - - # Get the vehicle speed at the time the first TCM message is received - speed_received_first_msg = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_received_first_msg): - speed_received_first_msg = msg.speed * 0.277777 # Conversion from kph to m/s - break - - # Get the route downtrack at the time the first TCM message is received - downtrack_received_first_msg = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_received_first_msg): - downtrack_received_first_msg = msg.down_track - break - - # Get the route downtrack at the time the geofence is entered - downtrack_enter_geofence = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_enter_geofence): - downtrack_enter_geofence = msg.down_track - break - - # Determine if the message was received early enough that the vehicle could decelerate in preparation for the - # geofence's advisory speed limit without surpassing the maximum deceleration limit - distance_for_decel = downtrack_enter_geofence - downtrack_received_first_msg - decel_required = ((speed_received_first_msg**2) - (advisory_speed_limit**2)) / (2*distance_for_decel) - if abs(decel_required) <= max_decel_limit: - has_decel_within_limit = True - - is_successful = False - if has_decel_within_limit: - print("B-3 succeeded; TCM message received with deceleration of " + str(decel_required) + " m/s^2 required before geofence") - is_successful = True - else: - print("B-3 failed; TCM message received with deceleration of " + str(decel_required) + " m/s^2 required before geofence.") - is_successful = False - - return is_successful - -def get_route_original_speed_limit(bag, time_test_start_engagement): - original_speed_limit = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_test_start_engagement): - original_speed_limit = msg.speed_limit - break - - return original_speed_limit - -########################################################################################################### -# Basic Travel B-5: The rear vehicle conducts a full lane merge by travelling through the following lanelets in order: -# (1) 96760, (2) 32982, (3) 34915 -########################################################################################################### -def check_lane_merge_before_geofence(bag): - lanelet_id_1 = 96760 - lanelet_id_2 = 32982 - lanelet_id_3 = 34915 - - travels_from_lanelet_1_to_2 = False - travels_from_lanelet_2_to_3 = False - previous_lanelet_id = 0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): - current_lanelet_id = msg.lanelet_id - - if previous_lanelet_id == lanelet_id_1 and current_lanelet_id == lanelet_id_2: - travels_from_lanelet_1_to_2 = True - - if travels_from_lanelet_1_to_2: - if previous_lanelet_id == lanelet_id_2 and current_lanelet_id == lanelet_id_3: - travels_from_lanelet_2_to_3 = True - break - - previous_lanelet_id = current_lanelet_id - - is_successful = False - if travels_from_lanelet_1_to_2 and travels_from_lanelet_2_to_3: - print("B-5 Succeeded: Vehicle travelled through the following lanelets in order: " + str(lanelet_id_1) + ", " + str(lanelet_id_2) + ", " + str(lanelet_id_3)) - is_successful = True - else: - print("B-5 Failed: Vehicle travelled through the following lanelets in order: " + str(lanelet_id_1) + ", " + str(lanelet_id_2) + ", " + str(lanelet_id_3)) - is_successful = False - - return is_successful - - -########################################################################################################### -# BASIC TRAVEL B-6: After the rear vehicle has completed its lane merge, the front vehicle will receive -# the 'Join Platoon at Rear' MobilityRequest message from the rear vehicle. The front -# vehicle shall respond with an 'Acknowledgement' MobilityRequestResponse message -# signaling acceptance. -########################################################################################################### -def check_join_at_rear_negotiation(bag, time_received_first_acceptance, time_last_sent_join_at_rear, time_start_engagement, start_lane_following_lanelet): - time_finish_lane_merge = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): - if msg.lanelet_id == start_lane_following_lanelet: - time_finish_lane_merge = t - break - - duration_first_acceptance_after_lane_merge = (time_received_first_acceptance - time_finish_lane_merge).to_sec() - duration_first_acceptance_after_join_at_rear = (time_received_first_acceptance - time_last_sent_join_at_rear).to_sec() - - is_successful = False - if duration_first_acceptance_after_join_at_rear > 0.0: - is_successful = True - print("B-6 succeeded; First acceptance received " + str(duration_first_acceptance_after_join_at_rear) + " sec after 'JOIN_PLATOON_AT_REAR") - if duration_first_acceptance_after_lane_merge > 0.0: - print("B-6 Note: First acceptance occurred " + str(round(duration_first_acceptance_after_lane_merge,2)) + " sec after end of lane merge") - else: - print("B-6 Note: First acceptance occurred " + str(round(-duration_first_acceptance_after_lane_merge,2)) + " sec before end of lane merge") - else: - is_successful = False - print("B-6 failed; First acceptance received " + str(duration_first_acceptance_after_join_at_rear) + " sec after 'JOIN_PLATOON_AT_REAR") - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-7: After the rear vehicle receives the 'Acknowledgement' MobilityRequestResponse message -# from the front vehicle signaling its acceptance for the rear vehicle to join its -# platoon, the rear vehicle shall respond with a 'Platoon Follower Join' MobilityRequest -# message to the front vehicle signaling that it is joining the front vehicle's platoon. -########################################################################################################### -def check_follower_join_negotiation(time_received_first_acceptance, time_last_sent_follower_join): - duration_follower_join_after_first_acceptance = (time_last_sent_follower_join - time_received_first_acceptance).to_sec() - - is_successful = False - if duration_follower_join_after_first_acceptance > 0.0: - is_successful = True - print("B-7 succeeded; 'PLATOON_FOLLOWER_JOIN' broadcasted " + str(duration_follower_join_after_first_acceptance) + " sec after first acceptance.") - else: - is_successful = False - print("B-7 failed; 'PLATOON_FOLLOWER_JOIN' broadcasted " + str(duration_follower_join_after_first_acceptance) + " sec after first acceptance.") - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-8: When the rear vehicle broadcasts its 'Platoon Follower Join' MobilityRequest message -# (the message signaling that it is joining the platoon), the time headway between the -# lead vehicle and the rear vehicle shall be +/- 2.0 seconds of the desired 2.5 second time headway. -########################################################################################################### -def check_time_headway_after_platoon_negotiation(bag, time_received_second_acceptance): - desired_time_headway = 2.5 # (seconds) Desired time headway between rear vehicle and front vehicle during platooning - threshold_time_headway = 2.0 # (seconds) Allowable time headway offset from the desired value at initial platoon formation - min_time_headway = desired_time_headway - threshold_time_headway # (seconds) - max_time_headway = desired_time_headway + threshold_time_headway # (seconds) - - # Obtain the distance headway between the rear vehicle and the front vehicle when the rear vehicle first becomes a 'Follower' - has_found_actual_time_headway_at_platoon_start = False - actual_gap = 0.0 - duration_since_second_acceptance = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_received_second_acceptance): - if msg.state == 5: - if (msg.actual_gap > 0): - actual_speed = msg.desired_gap / desired_time_headway - actual_time_headway = msg.actual_gap / actual_speed - has_found_actual_time_headway_at_platoon_start = True - - # Obtain the duration between this first occurrence of being a 'Follower' and when the second acceptance message was received - duration_since_second_acceptance = (t - time_received_second_acceptance).to_sec() - break - - is_successful = False - if has_found_actual_time_headway_at_platoon_start: - # Successful if the time headway at platooning start is within the minimum and maximum values - if (min_time_headway <= actual_time_headway <= max_time_headway): - print("B-8 succeeded; Time headway at start of platooning is " + str(actual_time_headway) + " sec (" + str(duration_since_second_acceptance) + " sec after 2nd acceptance)") - is_successful = True - else: - print("B-8 failed; Time headway at start of platooning is " + str(actual_time_headway) + " sec (" + str(duration_since_second_acceptance) + " sec after 2nd acceptance)") - is_successful = False - else: - print("B-8 failed; Rear vehicle never published PlatooningInfo msg with 'FOLLOWING' status") - is_successful = False - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-9: After platoon negotiation is complete, the rear vehicle changes its platooning status -# from 'CandidateFollower' to 'Follower'. -########################################################################################################### -def check_follower_state_after_platoon_negotiation(bag, time_last_sent_join_at_rear): - has_candidate_follower_state = False # Flag to track whether there has been a 'CandidateFollower' state - has_follower_state = False # Flag to track whether there has been a 'Follower' state - duration_since_last_sent_join_at_rear = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_last_sent_join_at_rear): - # If a 'CandidateFollower' state occurs for the first time, set the appropriate flag to True - if not has_candidate_follower_state: - if msg.state == 3: - has_candidate_follower_state = True - - # If a 'Follower' state occurs for the first time, set the appropriate flag to True - elif not has_follower_state: - if msg.state == 5: - # Obtain the time duration between the start of the 'Follower' state and time_last_sent_join_at_rear - duration_since_last_sent_join_at_rear = (t - time_last_sent_join_at_rear).to_sec() - has_follower_state = True - break - - is_successful = False - if has_candidate_follower_state and has_follower_state: - print("B-9 successful; Rear vehicle transitioned from CANDIDATEFOLLOWER to FOLLOWER (" + str(duration_since_last_sent_join_at_rear) + " sec after 'JOIN_PLATOON_AT_REAR' msg)") - is_successful = True - elif has_candidate_follower_state: - print("B-9 failed; Rear vehicle never transitioned to FOLLOWER") - is_successful = False - else: - print("B-9 failed; Rear vehicle never transitioned to CANDIDATEFOLLOWER") - is_successful = False - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-10: After platoon negotiation is complete, the lead vehicle changes its platooning status -# from 'LeaderWaiting' to 'Leader'. -########################################################################################################### -def check_leader_state_after_platoon_negotiation(bag, time_last_received_join_at_rear): - has_leader_waiting_state = False # Flag to track whether there has been a 'LeaderWaiting' state - has_leader_state = False # Flag to track whether there has been a 'Leader' state - duration_since_last_received_join_at_rear = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_last_received_join_at_rear): - # If this is the first occurrence of a 'LeaderWaiting' state, set the appropriate flag - if not has_leader_waiting_state: - if msg.state == 2: - has_leader_waiting_state = True - # If this is the first occurrence of a 'Leader' state, set the appropriate flag - elif not has_leader_state: - if msg.state == 4: - duration_since_last_received_join_at_rear = (t - time_last_received_join_at_rear).to_sec() - has_leader_state = True - break - - # Successful if both flags are true - is_successful = False - if has_leader_waiting_state and has_leader_state: - print("B-10 successful; Front vehicle transitioned from LEADERWAITING to LEADER (" + str(duration_since_last_received_join_at_rear) + " sec after 'JOIN_PLATOON_AT_REAR' msg)") - is_successful = True - elif has_leader_waiting_state: - print("B-10 failed; Front vehicle never transitioned to LEADER") - is_successful = False - else: - print("B-10 failed; Front vehicle never transitioned to LEADERWAITING") - is_successful = False - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-11: During platooning operations, the time gap between the front vehicle and the rear -# vehicle shall always be +/- 0.5 seconds of the desired 2.5 second time gap. -########################################################################################################### -def check_distance_gap_during_platooning(bag, time_received_second_acceptance, time_end_engagement): - # Parameters used for this metric - threshold_time_gap = 0.5 # (seconds) Threshold offset from desired time gap; if the actual time gap is within this threshold, it is considered successful - desired_time_gap = 2.5 # (seconds) The absolute desired time gap between the Follower vehicle and the Leader vehicle during platooning operations - min_time_gap = desired_time_gap - threshold_time_gap # The minimum allowable time gap between the Follower vehicle and the Leader vehicle during platooning operations - max_time_gap = desired_time_gap + threshold_time_gap # The maximum allowable time gap between the Follower vehicle and the Leader vehicle during platooning operations - - # Variables that are tracked for this metric - count_follower_msgs = 0 - count_negative_gap_msgs = 0 - lowest_time_gap = 100.0 - largest_time_gap = -100.0 - count_below_min_time_gap_msgs = 0 - count_above_max_time_gap_msgs = 0 - count_successful_time_gap_msgs = 0 - prev_negative_t = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_received_second_acceptance, end_time = time_end_engagement): - # Only consider messages when the vehicle's platooning state is 'FOLLOWER' - if msg.state == 5: - if msg.actual_gap < 0: - count_negative_gap_msgs += 1 - duration_since_prev_neg = (t - prev_negative_t).to_sec() - if duration_since_prev_neg >= 0.5: - print("Time since previous negative actual gap: " + str(duration_since_prev_neg) + " seconds") - prev_negative_t = t - print("Received negative actual gap at time " + str(t.to_sec())) - if count_negative_gap_msgs == 20: - break - else: - actual_speed = msg.desired_gap / desired_time_gap - actual_time_gap = msg.actual_gap / actual_speed - if (actual_time_gap < min_time_gap): - #print("Found time headway below 2.0") - count_below_min_time_gap_msgs += 1 - if actual_time_gap < lowest_time_gap: - lowest_time_gap = actual_time_gap - elif (actual_time_gap > max_time_gap): - #print("Found time headway above 3.0 " + str((t - time_received_second_acceptance).to_sec()) + " sec after negotiation") - count_above_max_time_gap_msgs += 1 - if actual_time_gap > largest_time_gap: - largest_time_gap = actual_time_gap - else: - count_successful_time_gap_msgs += 1 - - count_follower_msgs += 1 - - pct_negative_gap_msgs = (float(count_negative_gap_msgs) / float(count_follower_msgs)) * 100.0 - pct_below_min_time_gap_msgs = (float(count_below_min_time_gap_msgs) / float(count_follower_msgs)) * 100.0 - pct_above_max_time_gap_msgs = (float(count_above_max_time_gap_msgs) / float(count_follower_msgs)) * 100.0 - pct_successful_time_gap_msgs = (float(count_successful_time_gap_msgs) / float(count_follower_msgs)) * 100.0 - - is_successful = False - if pct_successful_time_gap_msgs == 1: - print("B-11 succeeded (100% Successful): Statistics below:") - is_successful = True - else: - print("B-11 failed (Not 100% Successful); Statistics below:") - is_successful = False - - print(str(count_follower_msgs) + " Follower Messages; " + str(count_negative_gap_msgs) + " Negative (" + str(round(pct_negative_gap_msgs,2)) + "%); " \ - + str(count_below_min_time_gap_msgs) + " Below (" + str(round(pct_below_min_time_gap_msgs,2)) + "%); " + str(count_above_max_time_gap_msgs) + " Above (" \ - + str(round(pct_above_max_time_gap_msgs,2)) + "%); " + str(count_successful_time_gap_msgs) + " Successful (" + str(round(pct_successful_time_gap_msgs,2)) + "%)") - if count_above_max_time_gap_msgs > 0: - print("B-11 Stat: Largest time gap: " + str(largest_time_gap)) - if count_below_min_time_gap_msgs > 0: - print("B-11 Stat: Smallest time gap: " + str(lowest_time_gap)) - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-12: (FOLLOWER VEHICLE) During platooning operations, the front 'Leader' vehicle shall -# continuously broadcast 'STATUS' platooning MobilityOperation messages to the rear 'Follower' -# vehicle with a time gap between sequentially broadcasted messages below 0.2 seconds -# at least 90% of the time. -########################################################################################################### -def check_percentage_successful_status_msg_follower(bag, time_received_second_acceptance): - # Parameters used for the computation of this metric - min_percent_time_successful = 90.0 # (90%); Percent of active platooning time that the minimum 'STATUS' frequency must be achieved - max_duration_between_status_msgs = 0.20 # (Seconds); Minimum duration between sequentially broadcasted 'STATUS' messages - - - # Obtain the timestamp of the last received MobilityOperation message from the leader. This is considered the end of - # platooning since the leader has disengaged. - time_last_received_mob_op = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_operation'], start_time = time_received_second_acceptance): - time_last_received_mob_op = t - - # Obtain the quantity of 'STATUS' MobilityOperation messages received during platooning operations - is_first_status_msg = True - time_sent_prev_status_msg = rospy.Time() - duration_since_prev_status_msg = 0.0 - total_time_between_unsuccessful_status_msgs = 0.0 - total_time_between_successful_status_msgs = 0.0 - total_time_spent_platooning = 0.0 - largest_duration_between_status_msgs = 0.0 - count_status_msgs = 0 - count_status_msgs_above_max_duration = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_received_second_acceptance, end_time = time_last_received_mob_op): - if "STATUS" in msg.strategy_params: - if is_first_status_msg: - time_sent_prev_status_msg = t - is_first_status_msg = False - continue - else: - duration_since_prev_status_msg = (t - time_sent_prev_status_msg).to_sec() - - # Track the number of messages that are broadcasted after the (1/min_frequency) duration just for debugging purposes - if (duration_since_prev_status_msg >= max_duration_between_status_msgs): - #print("Duration of " + str(duration_since_prev_status_msg) + " at " + str(t.to_sec()) + " sec") - total_time_between_unsuccessful_status_msgs += duration_since_prev_status_msg - count_status_msgs_above_max_duration += 1 - if duration_since_prev_status_msg > largest_duration_between_status_msgs: - largest_duration_between_status_msgs = duration_since_prev_status_msg - else: - total_time_between_successful_status_msgs += duration_since_prev_status_msg - total_time_spent_platooning += duration_since_prev_status_msg - time_sent_prev_status_msg = t - count_status_msgs += 1 - - percent_time_successful = (total_time_between_successful_status_msgs / total_time_spent_platooning) * 100.0 - - # Update is_successful flag and print debug statements - is_successful = False - if percent_time_successful >= min_percent_time_successful: - print("B-12 Succeeded; Time between messages was below " + str(round(max_duration_between_status_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = True - else: - print("B-12 Failed; Time between messages was below " + str(round(max_duration_between_status_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = False - - pct_above_max_duration = (float(count_status_msgs_above_max_duration) / float(count_status_msgs)) * 100.0 - print(str(count_status_msgs) + " Total STATUS Messages sent (" + str(round(100-pct_above_max_duration,3)) + "% Successful); " + str(count_status_msgs_above_max_duration) + " after " + str(round(max_duration_between_status_msgs,3)) + " sec (" \ - + str(round(pct_above_max_duration,3)) + "%);; Longest duration " \ - + " between STATUS messages was " + str(largest_duration_between_status_msgs) + " sec") - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-12: (LEADER VEHICLE) During platooning operations, the front 'Leader' vehicle shall -# continuously broadcast 'STATUS' platooning MobilityOperation messages to the rear 'Follower' -# vehicle with a time gap between sequentially broadcasted messages below 0.2 seconds -# at least 90% of the time. -########################################################################################################### -def check_percentage_successful_status_msg_leader(bag, time_sent_second_acceptance): - # Parameters used for the computation of this metric - min_percent_time_successful = 90.0 # (90%); Percent of active platooning time that the minimum 'STATUS' frequency must be achieved - max_duration_between_status_msgs = 0.20 # (Seconds); Minimum duration between sequentially broadcasted 'STATUS' messages - - # Obtain the timestamp of the last broadcasted MobilityOperation message from the leader. This is considered the end of - # platooning since the leader has disengaged. - time_last_sent_mob_op = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance): - time_last_sent_mob_op = t - print("Leader sent last MobilityOperation at " + str(time_last_sent_mob_op.to_sec())) - - # Obtain the quantity of 'STATUS' MobilityOperation messages broadcasted during platooning operations - is_first_status_msg = True - time_sent_prev_status_msg = rospy.Time() - duration_since_prev_status_msg = 0.0 - total_time_between_unsuccessful_status_msgs = 0.0 - total_time_between_successful_status_msgs = 0.0 - total_time_spent_platooning = 0.0 - largest_duration_between_status_msgs = 0.0 - count_status_msgs = 0 - count_status_msgs_above_max_duration = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance, end_time = time_last_sent_mob_op): - if "STATUS" in msg.strategy_params: - if is_first_status_msg: - time_sent_prev_status_msg = t - is_first_status_msg = False - continue - else: - duration_since_prev_status_msg = (t - time_sent_prev_status_msg).to_sec() - - # Track the number of messages that are broadcasted after the (1/min_frequency) duration just for debugging purposes - if (duration_since_prev_status_msg >= max_duration_between_status_msgs): - #print("Duration of " + str(duration_since_prev_status_msg) + " at " + str(t.to_sec()) + " sec") - total_time_between_unsuccessful_status_msgs += duration_since_prev_status_msg - count_status_msgs_above_max_duration += 1 - if duration_since_prev_status_msg > largest_duration_between_status_msgs: - largest_duration_between_status_msgs = duration_since_prev_status_msg - else: - total_time_between_successful_status_msgs += duration_since_prev_status_msg - total_time_spent_platooning += duration_since_prev_status_msg - time_sent_prev_status_msg = t - count_status_msgs += 1 - - percent_time_successful = (total_time_between_successful_status_msgs / total_time_spent_platooning) * 100.0 - - # Update is_successful flag and print debug statements - is_successful = False - if percent_time_successful >= min_percent_time_successful: - print("B-12 Succeeded; Time between messages was below " + str(round(max_duration_between_status_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = True - else: - print("B-12 Failed; Time between messages was below " + str(round(max_duration_between_status_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = False - - pct_above_max_duration = (float(count_status_msgs_above_max_duration) / float(count_status_msgs)) * 100.0 - print(str(count_status_msgs) + " Total STATUS Messages sent (" + str(round(100-pct_above_max_duration,3)) + "% Successful); " + str(count_status_msgs_above_max_duration) + " after " + str(round(max_duration_between_status_msgs,3)) + " sec (" \ - + str(round(pct_above_max_duration,3)) + "%);; Longest duration " \ - + " between STATUS messages was " + str(largest_duration_between_status_msgs) + " sec") - - return is_successful - -########################################################################################################### -# Basic Travel B-13: The executed trajectory to prepare for the geofence will include a deceleration section and -# the average deceleration amount shall be no less than 1 m/s^2. -########################################################################################################### -def check_deceleration_before_geofence(bag, time_enter_geofence, original_speed_limit, advisory_speed_limit): - # Parameters used for metric evaluation - min_average_deceleration = 1.0 # m/s^2 - start_decel_percent_of_original_speed_limit = 0.90 # Percentage (0.90 is 90%) of original speed limit for current speed to be considered the start of deceleration - end_decel_percent_of_advisory_speed_limit = 1.10 # Percentage (1.10 is 110%) of advisory speed limit for current speed to be considered the end of deceleration - - # Variables to track during metric evaluation - speed_start_decel = 0.0 - speed_end_decel = 0.0 - time_start_decel = rospy.Time() - time_end_decel = rospy.Time() - has_found_start_of_decel = False - has_found_end_of_decel = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_enter_geofence): - if has_found_start_of_decel: - decel = (msg.twist.linear.x - prev_speed) / (t-prev_time).to_sec() - #print("Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x) + "; " + str(decel) + " m/s^2") - prev_speed = msg.twist.linear.x - prev_time = t - - # Get start time of deceleration - if not has_found_start_of_decel and (msg.twist.linear.x < (start_decel_percent_of_original_speed_limit * original_speed_limit)): - print("Start Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_start_decel = t - speed_start_decel = msg.twist.linear.x - has_found_start_of_decel = True - prev_speed = msg.twist.linear.x - prev_time = t - - # Get end time of deceleration - if has_found_start_of_decel and (msg.twist.linear.x < (end_decel_percent_of_advisory_speed_limit * advisory_speed_limit)): - print("End Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_end_decel = t - speed_end_decel = msg.twist.linear.x - has_found_end_of_decel = True - break - - deceleration_completed_before_geofence = False - if (time_enter_geofence-time_end_decel).to_sec() > 0: - deceleration_completed_before_geofence = True - - is_successful = False - if has_found_start_of_decel and has_found_end_of_decel: - - # Calculate the average deceleration rate during the deceleration phase - average_deceleration = (speed_start_decel - speed_end_decel) / (time_end_decel - time_start_decel).to_sec() - - if average_deceleration >= min_average_deceleration and deceleration_completed_before_geofence: - print("B-13 succeeded; average deceleration above 1.0 m/s^2 occurred before geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = True - elif average_deceleration >= min_average_deceleration and not deceleration_completed_before_geofence: - print("B-13 failed; average deceleration above 1.0 m/s^2 occurred after geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = False - elif average_deceleration <= min_average_deceleration and deceleration_completed_before_geofence: - print("B-13 failed; a deceleration below 1.0 m/s^2 occurred before the geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = False - elif average_deceleration <= min_average_deceleration and not deceleration_completed_before_geofence: - print("B-13 failed; a deceleration below 1.0 m/s^2 occurred after the geofence entrance: " + str(average_deceleration) + " m/s^2") - is_successful = False - elif has_found_start_of_decel: - print("B-13 failed; did not find end of deceleration phase") - is_successful = False - else: - print("B-13 failed; did not find start of deceleration phase") - is_successful = False - - return is_successful - -########################################################################################################### -# Basic Travel B-14: The executed trajectory will start calling for acceleration back to the original speed limit -# no more than 30 feet away from the end of the geo-fenced area. -########################################################################################################### -def check_acceleration_distance_after_geofence(bag, time_exit_geofence): - max_distance_from_geofence_end = 30.0 # (feet) Max distance from end of geofence for first lane change point - min_distance_from_geofence_end = 0.0 # (feet) Minimum distance from end of geofence for first lane change point - required_sequential_speed_increases = 5 # The number of sequential speed increases required to be considered the start of acceleration - conversion_meters_to_feet = 3.28084 # 1 meter is 3.28084 feet - - # Get the downtrack at the end of the geofence - downtrack_exit_geofence = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence): - downtrack_exit_geofence = msg.down_track - break - - # Variables to track during metric evaluation - has_found_start_of_accel = False - prev_speed = 0.0 - prev_t = rospy.Time() - has_found_start_of_accel = False - time_start_of_accel = rospy.Time() - is_first_speed = True - count_speed_increases = 0 - - # Get the timestamp associated with the start of vehicle acceleration after exiting the geofence - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_exit_geofence): - if is_first_speed: - prev_speed = msg.twist.linear.x - prev_t = t - is_first_speed = False - continue - - # Get start time of acceleration - current_speed = msg.twist.linear.x - duration_since_previous_speed = (t - prev_t).to_sec() - current_accel = (current_speed - prev_speed) / duration_since_previous_speed - #print("Current acceleration: " + str(current_accel) + " m/s^2; speed is " + str(current_speed) + " m/s") - - # Check if this is the start of acceleration - if count_speed_increases == 0: - time_start_of_accel = t # Always track the start time of consecutive speed increases - - if current_speed > prev_speed: - count_speed_increases += 1 - else: - count_speed_increases = 0 - - # End loop if reached the required number of consecutive speed increases - if count_speed_increases == required_sequential_speed_increases: - has_found_start_of_accel = True - break - - # Update variables - prev_t = t - prev_speed = current_speed - - # Get the distance after the geofence that the vehicle begins accelerating if any acceleration was found - if has_found_start_of_accel: - # Get the downtrack at the start of acceleration back to the original speed limit - downtrack_start_of_accel = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_of_accel): - downtrack_start_of_accel = msg.down_track - break - - # Obtain the distance (feet) after the geofence that the vehicle begins accelerating - distance_start_accel_after_geofence = (downtrack_start_of_accel - downtrack_exit_geofence) * conversion_meters_to_feet - - # Set is_successful flag and print debug statements - is_successful = False - if has_found_start_of_accel: - if distance_start_accel_after_geofence >= min_distance_from_geofence_end and distance_start_accel_after_geofence <= max_distance_from_geofence_end: - print("B-14 succeeded; vehicle acceleration began " + str(distance_start_accel_after_geofence) + " feet after exiting the geofence.") - is_successful = True - else: - print("B-14 failed; vehicle acceleration began " + str(distance_start_accel_after_geofence) + " feet after exiting the geofence.") - is_successful = False - else: - print("B-14 failed; no vehicle acceleration occurred after exiting the geofence.") - is_successful = False - - return is_successful - -########################################################################################################### -# Basic Travel B-15: The executed trajectory back to normal operations will include an acceleration portion and -# the average acceleration over the entire acceleration time shall be no less than 1 m/s^2. -########################################################################################################### -def check_acceleration_rate_after_geofence(bag, time_exit_geofence, original_speed_limit, advisory_speed_limit): - # Parameters used for metric evaluation - min_average_acceleration = 1.0 # m/s^2 - end_accel_percent_of_original_speed_limit = 0.90 - start_accel_percent_of_advisory_speed_limit = 1.10 - - # Variables to track during metric evaluation - speed_start_accel = 0.0 - speed_end_accel = 0.0 - time_start_accel = rospy.Time() - time_end_accel = rospy.Time() - has_found_start_of_accel = False - has_found_end_of_accel = False - - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_exit_geofence): - if has_found_start_of_accel: - accel = (msg.twist.linear.x - prev_speed) / (t-prev_time).to_sec() - #print("Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x) + "; " + str(accel) + " m/s^2") - prev_speed = msg.twist.linear.x - prev_time = t - - # Get start time of deceleration - if not has_found_start_of_accel and (msg.twist.linear.x > (start_accel_percent_of_advisory_speed_limit * advisory_speed_limit)): - print("Accel Start Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_start_accel = t - speed_start_accel = msg.twist.linear.x - has_found_start_of_accel = True - prev_speed = msg.twist.linear.x - prev_time = t - - # Get end time of deceleration - if has_found_start_of_accel and (msg.twist.linear.x > (end_accel_percent_of_original_speed_limit * original_speed_limit)): - print("Accel End Time: " + str(t.to_sec()) + "; " + str(msg.twist.linear.x)) - time_end_accel = t - speed_end_accel = msg.twist.linear.x - has_found_end_of_accel = True - break - - # If the full acceleration phase has been found, determine the average acceleration for this phase - if has_found_start_of_accel and has_found_end_of_accel: - average_acceleration = (speed_end_accel - speed_start_accel) / (time_end_accel - time_start_accel).to_sec() - print("Avg acceleration: " + str(average_acceleration)) - - # Print success/failure statement and return success flag - is_successful = False - if (has_found_start_of_accel and has_found_end_of_accel): - if average_acceleration >= min_average_acceleration: - print("B-15 succeeded; average acceleration after geofence was above 1.0 m/s^2: " + str(average_acceleration) + " m/s^2") - is_successful = True - else: - print("B-15 failed; average acceleration after geofence was below 1.0 m/s^2: " + str(average_acceleration) + " m/s^2") - is_successful = False - elif has_found_start_of_accel: - print("B-15 failed; no end of acceleration after exiting the geofence was found.") - is_successful = True - else: - print("B-15 failed; no acceleration after exiting the geofence was found.") - is_successful = False - - return is_successful - -########################################################################################################### -# Basic Travel B-16: The planned route must end with the CP vehicle having been at steady state, after all other -# maneuvers, for at least 10 seconds. -########################################################################################################### -def check_steady_state_after_geofence(bag, time_exit_geofence, time_end_engagement, original_speed_limit): - # Parameters used for metric evaluation - # (m/s) Threshold offset from speed limit; vehicle considered at steady state when its speed is within this offset of the speed limit - threshold_speed_limit_offset = 0.894 # 0.894 m/s is 2 mph - # (m/s) Minimum speed to be considered at steady state - min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset - # (m/s) Maximum speed to be considered at steady state - max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset - # (seconds) Minimum required threshold at steady state after completing all geofence-triggered maneuvers - min_steady_state_time = 10.0 - - # Conduct steady state evaluation - # Get the start time of the vehicle reaching steady state (if one exists) - has_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_exit_geofence, end_time = time_end_engagement): - # Vehicle has reached steady state when its speed within threshold range of steady state speed - if (max_steady_state_speed >= msg.twist.linear.x >= min_steady_state_speed): - time_start_steady_state = t - has_steady_state = True - break - - # If the start time of steady state was found, find the longest duration of steady state time - has_passed_steady_state_time_threshold = False - max_steady_state_duration = 0.0 - if (has_steady_state): - has_passed_steady_state_time_threshold = False - is_at_steady_state = True - steady_state_duration = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_steady_state, end_time = time_end_engagement): - current_speed = msg.twist.linear.x - - # If system is entering steady state, reset the steady state start time: - if ((max_steady_state_speed >= current_speed >= min_steady_state_speed) and not is_at_steady_state): - is_at_steady_state = True - time_start_steady_state = t - - # If system is maintaining steady state, check if it has passed the threshold time of continuous steady state: - elif ((max_steady_state_speed >= current_speed >= min_steady_state_speed) and is_at_steady_state): - steady_state_duration = (t - time_start_steady_state).to_sec() - - if steady_state_duration > max_steady_state_duration: - max_steady_state_duration = steady_state_duration - - if (steady_state_duration >= min_steady_state_time): - has_passed_steady_state_time_threshold = True - - # If system has exited steady state, reset the steady state flag - elif ((max_steady_state_speed <= current_speed or current_speed <= min_steady_state_speed) and is_at_steady_state): - is_at_steady_state = False - - if (has_passed_steady_state_time_threshold): - print("B-16 succeeded; system reached continuous steady state for " + str(max_steady_state_duration) + " seconds after geofence-triggered maneuvers.") - is_successful = True - else: - if has_steady_state: - print("B-16 failed; system reached continuous steady state for " + str(steady_state_duration) + " seconds after geofence-triggered maneuvers. " \ - + " At least " + str(min_steady_state_time) + " seconds required.") - is_successful = False - if not has_steady_state: - print("B-16 failed; system did not reach steady state after geofence-triggered maneuvers.") - is_successful = False - - return is_successful - -########################################################################################################### -# Basic Travel B-17: The entire scenario will satisfy all previous criteria using any of the speeds given here for -# the "regular speed limit" (i.e. the speed limit when not in the geo-fence). -########################################################################################################### -def check_speed_limit_when_not_in_geofence(bag, time_start_engagement, time_enter_geofence, time_exit_geofence, time_end_engagement, original_speed_limit): - # (m/s) Threshold offset from speed limit to account for floating point precision - threshold_speed_limit_offset = 0.01 - max_speed_limit = original_speed_limit + threshold_speed_limit_offset # (m/s) - min_speed_limit = original_speed_limit - threshold_speed_limit_offset # (m/s) - - # Tolerance for the time between the geofence start and end and finding a speed limit that belongs in the geofence: - # Note: This tolerance is due to timing not being fully-synchronized between /environment/active_geofence and all other topics - time_tolerance_geofence = 0.80 # seconds - - # Check speed limit before entering geofence - is_correct_speed_limit_before_geofence = False - time_incorrect_speed_limit_before_geofence = 0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_enter_geofence): - # The first correct speed limit has been found - if ((max_speed_limit >= msg.speed_limit >= min_speed_limit) and not is_correct_speed_limit_before_geofence): - is_correct_speed_limit_before_geofence = True - # An incorrect speed limit has been found; will trigger a failure if not within tolerance of the geofence entrance - elif((msg.speed_limit >= max_speed_limit or msg.speed_limit <= min_speed_limit) and is_correct_speed_limit_before_geofence): - time_incorrect_speed_limit_before_geofence = abs(time_enter_geofence - t).to_sec() - - # Not a failure if within tolerance time of the geofence entrance - if (time_incorrect_speed_limit_before_geofence <= time_tolerance_geofence): - is_correct_speed_limit_before_geofence = True - continue - - # Failure if not within tolerance time of the geofence entrance - print("Speed limit " + str(msg.speed_limit) + " m/s found at " + str(t.to_sec()) + \ - ", which is " + str(time_incorrect_speed_limit_before_geofence) + " seconds before entering the geofence.") - is_correct_speed_limit_before_geofence = False - break - - # Check speed limit after exiting geofence - is_correct_speed_limit_after_geofence = False - time_incorrect_speed_limit_after_geofence = 0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence, end_time = time_end_engagement): - # Speed limit is correct for the first time after exiting the geofence - if ((max_speed_limit >= msg.speed_limit >= min_speed_limit) and not is_correct_speed_limit_after_geofence): - time_incorrect_speed_limit_after_geofence = abs(t - time_exit_geofence).to_sec() - if (time_incorrect_speed_limit_after_geofence <= time_tolerance_geofence): - is_correct_speed_limit_after_geofence = True - continue - - # Failure; speed limit is incorrect and not within tolerance time of exiting geofence: - elif((msg.speed_limit >= max_speed_limit or msg.speed_limit <= min_speed_limit) and not is_correct_speed_limit_after_geofence): - time_incorrect_speed_limit_after_geofence = abs(t - time_exit_geofence).to_sec() - print("Speed limit " + str(msg.speed_limit) + " m/s found at " + str(t.to_sec()) + \ - ", which is " + str(time_incorrect_speed_limit_after_geofence) + " seconds after exiting the geofence.") - is_correct_speed_limit_after_geofence = False - break - - # Failure; speed limit is incorrect after having been correct before - elif((msg.speed_limit >= max_speed_limit or msg.speed_limit <= min_speed_limit) and is_correct_speed_limit_after_geofence): - is_correct_speed_limit_after_geofence = False - break - - # Print success/failure statement and return success flag - if (not is_correct_speed_limit_before_geofence and not is_correct_speed_limit_after_geofence): - print("B-17 failed; speed limit was not always " + str(original_speed_limit) + " m/s before OR after the geofence.") - is_successful = False - elif(not is_correct_speed_limit_before_geofence): - print("B-17 failed; speed limit was not always " + str(original_speed_limit) + " m/s before the geofence.") - is_successful = False - elif(not is_correct_speed_limit_after_geofence): - print("B-17 failed; speed limit was not always " + str(original_speed_limit) + " m/s after the geofence.") - is_successful = False - else: - print("B-17 succeeded; speed limit was always " + str(original_speed_limit) + " m/s before AND after the geofence.") - is_successful = True - - return is_successful - -########################################################################################################### -# Basic Travel B-18: The advisory speed limit communicated by CARMA Cloud shall be 10 mph less than the -# regular speed limit. -########################################################################################################### -def check_advisory_speed_limit(bag, advisory_speed_limit, original_speed_limit): - # (m/s) Required offset from normal speed limit required for the advisory speed limit - speed_limit_offset = 4.4704 # 4.4704 m/s is 10 mph - # (m/s) Threshold offset from expected advisory speed limit to account for floating point precision - threshold_speed_limit_offset = 0.01 - max_advisory_speed_limit = (original_speed_limit - speed_limit_offset) + threshold_speed_limit_offset - min_advisory_speed_limit = (original_speed_limit - speed_limit_offset) - threshold_speed_limit_offset - - # Evaluate speed limits, print success/failure statement, and return success flag - advisory_speed_limit_mph = int(advisory_speed_limit * 2.23694) # Conversion of m/s to mph - original_speed_limit_mph = int(original_speed_limit * 2.23694) # Conversion of m/s to mph - speed_limit_offset_mph = int(speed_limit_offset * 2.23694) # Conversion of m/s to mph - if (max_advisory_speed_limit >= advisory_speed_limit >= min_advisory_speed_limit): - print("B-18 succeeded; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ - + " mph), which is " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ - + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") - is_successful = True - else: - print("B-18 failed; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ - + " mph), which is not " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ - + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") - is_successful = False - - return is_successful - -########################################################################################################### -# Basic Travel B-20 (Front Vehicle) and B-21 (Rear Vehicle): The vehicle is fully contained within its lane -# (it does not breach either of the lane lines) for at least 95% of the time spent with -# the CARMA system engaged. -########################################################################################################### -def check_vehicle_inside_lane(bag, time_start_engagement, time_end_engagement, is_front_vehicle): - # Parameters used for this metric evaulation - vehicle_half_width = 1.01 # (meters) for Chrysler Pacifica Minivan; 0.955 Ford Fusion; 0.947 Lexus - percent_required_inside_lane = 95.0 # (percentage; 0.0 to 100.0) - lane_width = 3.7 # (meters) - max_allowed_cross_track = (lane_width / 2.0) - vehicle_half_width - time_begin_evaluation_after_start_engagement = 5.0 # (seconds) Ensures we don't capture crosstrack error from driver manually positioning vehicle in poor position at start of test - time_start = rospy.Time(time_start_engagement.to_sec() + time_begin_evaluation_after_start_engagement) - - # Variables to track during metric evaluation - max_found_cross_track = 0.0 - time_outside_lane = 0.0 - time_inside_lane = 0.0 - prev_t = rospy.Time() - first = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start, end_time = time_end_engagement): - # For first message, just save its timestamp within the 'prev_t' object - if first: - prev_t = t - first = False - continue - - # If cross track indicates vehicle is outside its lane, update total amount of time spent outside of lane - if msg.cross_track > max_allowed_cross_track: - time_outside_lane += (t - prev_t).to_sec() - - # Debug Statement - #print("Time " + str(t.to_sec()) + ": Outside Lane CTE " + str(msg.cross_track) + " meters") - # If cross track indicates vehicle is inside its lane, update total amount of time spent inside of lane - else: - time_inside_lane += (t - prev_t).to_sec() - - # Debug Statement - #print("Time " + str(t.to_sec()) + ": Inside Lane CTE " + str(msg.cross_track) + " meters") - - if msg.cross_track > max_found_cross_track: - max_found_cross_track = msg.cross_track - - prev_t = t - - percent_inside_lane = (time_inside_lane / (time_inside_lane + time_outside_lane)) * 100.0 - max_dist_outside_lane = (max_found_cross_track + vehicle_half_width) - (lane_width / 2.0) - - is_successful = False - if percent_inside_lane >= percent_required_inside_lane: - if is_front_vehicle: - print("B-20 succeeded; vehicle was fully inside its lane for " + str(round(percent_inside_lane,4)) + "% of time spent engaged (>= 95%).") - else: - print("B-21 succeeded; vehicle was fully inside its lane for " + str(round(percent_inside_lane,4)) + "% of time spent engaged (>= 95%).") - print(str(round(time_inside_lane,4)) + " sec inside lane; " + str(round(time_outside_lane,4)) + " sec outside lane; max crosstrack: " + str(round(max_found_cross_track,4)) + "; max dist outside lane: " + str(round(max_dist_outside_lane,4)) + " m") - is_successful = True - else: - if is_front_vehicle: - print("B-20 failed; vehicle was fully inside its lane for " + str(round(percent_inside_lane,4)) + "% of time spent engaged (< 95%).") - else: - print("B-21 failed; vehicle was fully inside its lane for " + str(round(percent_inside_lane,4)) + "% of time spent engaged (< 95%).") - print(str(round(time_inside_lane,4)) + " sec inside lane; " + str(round(time_outside_lane,4)) + " sec outside lane; max crosstrack: " + str(round(max_found_cross_track,4)) + "; max dist outside lane: " + str(round(max_dist_outside_lane,4)) + " m") - is_successful = True - return is_successful - -########################################################################################################### -# Basic Travel B-22 (Front Vehicle) and B-23 (Rear Vehicle): The vehicle never breaches the left or right -# lane line associated with its current lane by more than 0.1 meters while the CARMA -# system is engaged. -########################################################################################################### -def check_vehicle_always_within_maximum_crosstrack(bag, time_start_engagement, time_end_engagement, is_front_vehicle): - # Parameters used for this metric evaulation - vehicle_half_width = 1.01 # (meters) for Chrysler Pacifica Minivan; 0.955 Ford Fusion; 0.947 Lexus - lane_width = 3.7 # (meters) - max_distance_over_lane_line = 0.1 # (meters) - max_allowed_cross_track = (lane_width / 2.0) - vehicle_half_width + max_distance_over_lane_line - time_begin_evaluation_after_start_engagement = 5.0 # (seconds) Ensures we don't capture crosstrack error from driver manually positioning vehicle in poor position at start of test - time_start = rospy.Time(time_start_engagement.to_sec() + time_begin_evaluation_after_start_engagement) - - # Variables to track during metric evaluation - breached_max_cross_track = False - max_found_cross_track = 0.0 - time_outside_allowable_cte = 0.0 - time_inside_allowable_cte = 0.0 - prev_t = rospy.Time() - first = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start, end_time = time_end_engagement): - # For first message, save its timestamp within the 'prev_t' object - if first: - prev_t = t - first = False - continue - - # If cross track indicates vehicle is outside its lane by more than allowable distance, update total amount of time spent outside of lane - if msg.cross_track > max_allowed_cross_track: - time_outside_allowable_cte += (t - prev_t).to_sec() - breached_max_cross_track = True - - # Debug Statement - #print("Time " + str(t.to_sec()) + ": Outside Lane CTE " + str(msg.cross_track) + " meters") - # If cross track indicates vehicle is note outside its lane by more than allowable distance, update total amount of time spent inside of lane - else: - time_inside_allowable_cte += (t - prev_t).to_sec() - - # Debug Statement - #print("Time " + str(t.to_sec()) + ": Inside Lane CTE " + str(msg.cross_track) + " meters") - - if msg.cross_track > max_found_cross_track: - max_found_cross_track = msg.cross_track - - prev_t = t - - percent_inside_lane = (time_inside_allowable_cte / (time_inside_allowable_cte + time_outside_allowable_cte)) * 100.0 - max_dist_outside_lane = (max_found_cross_track + vehicle_half_width) - (lane_width / 2.0) - - is_successful = False - if not breached_max_cross_track: - if is_front_vehicle: - print("B-22 succeeded; vehicle never breached lane lines by more than 0.1 meters.") - else: - print("B-23 succeeded; vehicle never breached lane lines by more than 0.1 meters.") - print(str(round(time_inside_allowable_cte,4)) + " sec not over line by 0.1 m; " + str(round(time_outside_allowable_cte,4)) + " sec over line by 0.1 m; max crosstrack: " + str(round(max_found_cross_track,4)) + "; max dist outside lane: " + str(round(max_dist_outside_lane,4)) + " m") - is_successful = True - else: - if is_front_vehicle: - print("B-22 failed; vehicle breached lane lines by more than 0.1 meters for " + str(round(percent_inside_lane,4)) + "% of time spent engaged (< 95%).") - else: - print("B-23 failed; vehicle breached lane lines by more than 0.1 meters for " + str(round(percent_inside_lane,4)) + "% of time spent engaged (< 95%).") - print(str(round(time_inside_allowable_cte,4)) + " sec not over line by 0.1 m; " + str(round(time_outside_allowable_cte,4)) + " sec ove rline by 0.1 m; max crosstrack: " + str(round(max_found_cross_track,4)) + "; max dist outside lane: " + str(round(max_dist_outside_lane,4)) + " m") - is_successful = True - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-24: (Leader VEHICLE) During platooning operations, the front 'Leader' vehicle shall -# continuously broadcast 'INFO' platooning MobilityOperation messages with a time gap -# between sequentially broadcasted messages below 0.5 seconds at least 90% of the time. -########################################################################################################### -def check_percentage_successful_info_msg(bag, time_sent_second_acceptance): - # Parameters used for the computation of this metric - min_percent_time_successful = 90.0 # (90%); Percent of active platooning time that the minimum 'INFO' frequency must be achieved - max_duration_between_info_msgs = 0.50 # (Seconds); Maximum duration between sequentially broadcasted 'INFO' messages - - # Obtain the timestamp of the last broadcasted MobilityOperation message from the leader. This is considered the end of - # platooning since the leader has disengaged. - time_last_sent_mob_op = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance): - time_last_sent_mob_op = t - print("Leader sent last MobilityOperation at " + str(time_last_sent_mob_op.to_sec())) - - # Obtain the quantity of 'INFO' MobilityOperation messages broadcasted during platooning operations - is_first_info_msg = True - time_sent_prev_info_msg = rospy.Time() - duration_since_prev_info_msg = 0.0 - total_time_between_unsuccessful_info_msgs = 0.0 - total_time_between_successful_info_msgs = 0.0 - total_time_spent_platooning = 0.0 - largest_duration_between_info_msgs = 0.0 - count_info_msgs = 0 - count_info_msgs_above_max_duration = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance, end_time = time_last_sent_mob_op): - if "INFO" in msg.strategy_params: - if is_first_info_msg: - time_sent_prev_info_msg = t - is_first_info_msg = False - continue - else: - duration_since_prev_info_msg = (t - time_sent_prev_info_msg).to_sec() - - # Track the number of messages that are broadcasted after the (1/min_frequency) duration just for debugging purposes - if (duration_since_prev_info_msg >= max_duration_between_info_msgs): - #print("Duration of " + str(duration_since_prev_info_msg) + " at " + str(t.to_sec()) + " sec") - total_time_between_unsuccessful_info_msgs += duration_since_prev_info_msg - count_info_msgs_above_max_duration += 1 - if duration_since_prev_info_msg > largest_duration_between_info_msgs: - largest_duration_between_info_msgs = duration_since_prev_info_msg - else: - total_time_between_successful_info_msgs += duration_since_prev_info_msg - total_time_spent_platooning += duration_since_prev_info_msg - time_sent_prev_info_msg = t - count_info_msgs += 1 - - percent_time_successful = (total_time_between_successful_info_msgs / total_time_spent_platooning) * 100.0 - - # Update is_successful flag and print debug statements - is_successful = False - if percent_time_successful >= min_percent_time_successful: - print("B-24 Succeeded; Time between messages was below " + str(round(max_duration_between_info_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = True - else: - print("B-24 Failed; Time between messages was below " + str(round(max_duration_between_info_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = False - - pct_above_max_duration = (float(count_info_msgs_above_max_duration) / float(count_info_msgs)) * 100.0 - print(str(count_info_msgs) + " Total INFO Messages sent (" + str(round(100-pct_above_max_duration,3)) + "% Successful); " + str(count_info_msgs_above_max_duration) + " after " + str(round(max_duration_between_info_msgs,3)) + " sec (" \ - + str(round(pct_above_max_duration,3)) + "%);; Longest duration " \ - + " between INFO messages was " + str(largest_duration_between_info_msgs) + " sec") - -########################################################################################################### -# BASIC TRAVEL B-25: After the rear vehicle responds to the front vehicle with its 'Platoon Follower Join' -# MobilityRequest message (the message signaling that it is joining the platoon), it shall -# take no more than 10 seconds for the rear vehicle to achieve a time headway with the front -# vehicle that is within 0.5 seconds of the desired 2.5 second time headway. -########################################################################################################### -def check_duration_before_successful_time_headway(bag, time_received_second_acceptance, time_enter_geofence): - threshold_time_gap = 0.5 # (seconds) Threshold offset from desired time gap; if the actual time gap is within this threshold, it is considered successful - desired_time_gap = 2.5 # (seconds) The absolute desired time gap between the Follower vehicle and the Leader vehicle during platooning operations - min_time_gap = desired_time_gap - threshold_time_gap # The minimum allowable time gap between the Follower vehicle and the Leader vehicle during platooning operations - max_time_gap = desired_time_gap + threshold_time_gap # The maximum allowable time gap between the Follower vehicle and the Leader vehicle during platooning operations - max_duration_before_successful_time_gap = 10.0 # (seconds) - - has_unsuccessful_time_gap = True - time_first_steady_state_successful_time_gap = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_received_second_acceptance, end_time = time_enter_geofence): - # Only consider messages when the vehicle's platooning state is 'FOLLOWER' - if msg.state == 5: - # Obtain the current time headway - actual_speed = msg.desired_gap / desired_time_gap - actual_time_gap = msg.actual_gap / actual_speed - - # Set flag if the current time headway is outside the desired range - if (actual_time_gap < min_time_gap or actual_time_gap > max_time_gap): - has_unsuccessful_time_gap = True - - # Otherwise, get the time of the first time headway that is not outside the desired range - # Note: This must be after the last time headway that is outside the desired range - else: - if has_unsuccessful_time_gap: - time_first_steady_state_successful_time_gap = t - has_unsuccessful_time_gap = False - - duration_before_successful_time_headway = (time_first_steady_state_successful_time_gap - time_received_second_acceptance).to_sec() - - is_successful = False - if duration_before_successful_time_headway <= max_duration_before_successful_time_gap: - print("B-25 Succeeded; achieved steady state time headway within desired range " + str(duration_before_successful_time_headway) + " sec after platoon negotiation (<= 10 sec).") - is_successful = True - else: - print("B-25 Failed; achieved steady state time headway within desired range " + str(duration_before_successful_time_headway) + " sec after platoon negotiation. (> 10 sec)") - is_successful = False - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-26: During platooning operations, the time gap between sequentially broadcasted 'STATUS' platooning -# MobilityOperation messages from the front 'Leader' vehicle to the rear 'Follower' vehicle -# shall never exceed 1 second. -########################################################################################################### -def check_max_time_between_status_msg(bag, time_sent_second_acceptance): - # Parameters used for the computation of this metric - max_percent_time_unsuccessful = 0.0 # (Percent) - max_duration_between_status_msgs = 1.0 # (Seconds); Minimum duration between sequentially broadcasted 'STATUS' messages - - # Obtain the timestamp of the last broadcasted MobilityOperation message from the leader. This is considered the end of - # platooning since the leader has disengaged. - time_last_sent_mob_op = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance): - time_last_sent_mob_op = t - print("Leader sent last MobilityOperation at " + str(time_last_sent_mob_op.to_sec())) - - # Obtain the quantity of 'STATUS' MobilityOperation messages broadcasted during platooning operations - is_first_status_msg = True - time_sent_prev_status_msg = rospy.Time() - duration_since_prev_status_msg = 0.0 - total_time_between_unsuccessful_status_msgs = 0.0 - total_time_between_successful_status_msgs = 0.0 - total_time_spent_platooning = 0.0 - largest_duration_between_status_msgs = 0.0 - count_status_msgs = 0 - count_status_msgs_above_max_duration = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance, end_time = time_last_sent_mob_op): - if "STATUS" in msg.strategy_params: - if is_first_status_msg: - time_sent_prev_status_msg = t - is_first_status_msg = False - continue - else: - duration_since_prev_status_msg = (t - time_sent_prev_status_msg).to_sec() - - # Track the number of messages that are broadcasted after the (1/min_frequency) duration just for debugging purposes - if (duration_since_prev_status_msg >= max_duration_between_status_msgs): - #print("Duration of " + str(duration_since_prev_status_msg) + " at " + str(t.to_sec()) + " sec") - total_time_between_unsuccessful_status_msgs += duration_since_prev_status_msg - count_status_msgs_above_max_duration += 1 - if duration_since_prev_status_msg > largest_duration_between_status_msgs: - largest_duration_between_status_msgs = duration_since_prev_status_msg - else: - total_time_between_successful_status_msgs += duration_since_prev_status_msg - total_time_spent_platooning += duration_since_prev_status_msg - time_sent_prev_status_msg = t - count_status_msgs += 1 - - percent_time_unsuccessful = (total_time_between_unsuccessful_status_msgs / total_time_spent_platooning) * 100.0 - - # Update is_successful flag and print debug statements - is_successful = False - if percent_time_unsuccessful <= max_percent_time_unsuccessful: - print("B-26 Succeeded; Time between STATUS messages was above " + str(round(max_duration_between_status_msgs,3)) + " seconds " + str(round(percent_time_unsuccessful,3)) + \ - "% of the time. (Must be lower than " + str(round(max_percent_time_unsuccessful,3)) + "%)") - is_successful = True - else: - print("B-26 Failed; Time between STATUS messages was above " + str(round(max_duration_between_status_msgs,3)) + " seconds " + str(round(percent_time_unsuccessful,3)) + \ - "% of the time. (Must be lower than " + str(round(max_percent_time_unsuccessful,3)) + "%)") - is_successful = False - - pct_above_max_duration = (float(count_status_msgs_above_max_duration) / float(count_status_msgs)) * 100.0 - print(str(count_status_msgs) + " Total STATUS Messages sent (" + str(round(100-pct_above_max_duration,3)) + "% Successful); " + str(count_status_msgs_above_max_duration) + " after " + str(round(max_duration_between_status_msgs,3)) + " sec (" \ - + str(round(pct_above_max_duration,3)) + "%);; Longest duration " \ - + " between STATUS messages was " + str(largest_duration_between_status_msgs) + " sec") - - return is_successful - -########################################################################################################### -# BASIC TRAVEL B-27: During platooning operations, the time gap between sequentially broadcasted 'Follower' platooning -# MobilityOperation messages from the front 'Leader' vehicle to the rear 'Follower' vehicle -# shall never exceed 1 second. -########################################################################################################### -def check_max_time_between_info_msg(bag, time_sent_second_acceptance): - # Parameters used for the computation of this metric - max_percent_time_unsuccessful = 0.0 # (Percent) - max_duration_between_info_msgs = 1.0 # (Seconds); Minimum duration between sequentially broadcasted 'INFO' messages - - # Obtain the timestamp of the last broadcasted MobilityOperation message from the leader. This is considered the end of - # platooning since the leader has disengaged. - time_last_sent_mob_op = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance): - time_last_sent_mob_op = t - print("Leader sent last MobilityOperation at " + str(time_last_sent_mob_op.to_sec())) - - # Obtain the quantity of 'INFO' MobilityOperation messages broadcasted during platooning operations - is_first_info_msg = True - time_sent_prev_info_msg = rospy.Time() - duration_since_prev_info_msg = 0.0 - total_time_between_unsuccessful_info_msgs = 0.0 - total_time_between_successful_info_msgs = 0.0 - total_time_spent_platooning = 0.0 - largest_duration_between_info_msgs = 0.0 - count_info_msgs = 0 - count_info_msgs_above_max_duration = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_sent_second_acceptance, end_time = time_last_sent_mob_op): - if "INFO" in msg.strategy_params: - if is_first_info_msg: - time_sent_prev_info_msg = t - is_first_info_msg = False - continue - else: - duration_since_prev_info_msg = (t - time_sent_prev_info_msg).to_sec() - - # Track the number of messages that are broadcasted after the (1/min_frequency) duration just for debugging purposes - if (duration_since_prev_info_msg >= max_duration_between_info_msgs): - #print("Duration of " + str(duration_since_prev_info_msg) + " at " + str(t.to_sec()) + " sec") - total_time_between_unsuccessful_info_msgs += duration_since_prev_info_msg - count_info_msgs_above_max_duration += 1 - if duration_since_prev_info_msg > largest_duration_between_info_msgs: - largest_duration_between_info_msgs = duration_since_prev_info_msg - else: - total_time_between_successful_info_msgs += duration_since_prev_info_msg - total_time_spent_platooning += duration_since_prev_info_msg - time_sent_prev_info_msg = t - count_info_msgs += 1 - - percent_time_unsuccessful = (total_time_between_unsuccessful_info_msgs / total_time_spent_platooning) * 100.0 - - # Update is_successful flag and print debug statements - is_successful = False - if percent_time_unsuccessful <= max_percent_time_unsuccessful: - print("B-27 Succeeded; Time between messages was above " + str(round(max_duration_between_info_msgs,3)) + " seconds " + str(round(percent_time_unsuccessful,3)) + \ - "% of the time. (Must be lower than " + str(round(max_percent_time_unsuccessful,3)) + "%)") - is_successful = True - else: - print("B-27 Failed; Time between messages was above " + str(round(max_duration_between_info_msgs,3)) + " seconds " + str(round(percent_time_unsuccessful,3)) + \ - "% of the time. (Must be lower than " + str(round(max_percent_time_unsuccessful,3)) + "%)") - is_successful = False - - pct_above_max_duration = (float(count_info_msgs_above_max_duration) / float(count_info_msgs)) * 100.0 - print(str(count_info_msgs) + " Total INFO Messages sent (" + str(round(100-pct_above_max_duration,3)) + "% Successful); " + str(count_info_msgs_above_max_duration) + " after " + str(round(max_duration_between_info_msgs,3)) + " sec (" \ - + str(round(pct_above_max_duration,3)) + "%);; Longest duration " \ - + " between INFO messages was " + str(largest_duration_between_info_msgs) + " sec") - - return is_successful - -# Main Function; run all tests from here -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - source_folder = sys.argv[1] - - # Re-direct the output of print() to a specified .txt file: - orig_stdout = sys.stdout - current_time = datetime.datetime.now() - text_log_filename = "Results_" + str(current_time) + ".txt" - text_log_file_writer = open(text_log_filename, 'w') - sys.stdout = text_log_file_writer - - # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - csv_results_filename = "Results_" + str(current_time) + ".csv" - csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Vehicle Role", - "BT-B-1 Result", "BT-B-2 Result", "BT-B-3 Result", "BT-B-4 Result", "BT-B-5 Result", "BT-B-6 Result", - "BT-B-7 Result", "BT-B-8 Result", "BT-B-9 Result", "BT-B-10 Result","BT-B-11 Result", "BT-B-12 Result", - "BT-B-13 Result", "BT-B-14 Result", "BT-B-15 Result", "BT-B-16 Result", "BT-B-17 Result", "BT-B-18 Result", - "BT-19 Result", "BT-20 Result", "BT-21 Result", "BT-22 Result", "BT-23 Result", "BT-24 Result", "BT-25 Result", - "BT-26 Result", "BT-27 Result"]) - - # Create list of Basic Travel White Pacifica (Leader) bag files to be processed - BT_leader_white_pacifica_bag_files = ["_2021-07-21-21-14-34_down-selected.bag", - "_2021-07-21-21-29-32_down-selected.bag", - "_2021-07-21-21-38-21_down-selected.bag", - "_2021-07-21-21-48-21_down-selected.bag", - "_2021-07-22-13-22-00_down-selected.bag", - "_2021-07-22-13-30-47_down-selected.bag", - "_2021-07-22-13-41-35_down-selected.bag", - "_2021-07-22-13-51-01_down-selected.bag", - "_2021-07-22-14-01-37_down-selected.bag", - "_2021-07-22-14-10-12_down-selected.bag", - "_2021-07-22-14-21-18_down-selected.bag", - "_2021-07-22-15-10-36_down-selected.bag", - "_2021-07-22-15-20-10_down-selected.bag", - "_2021-07-22-15-30-47_down-selected.bag", - "_2021-07-22-15-41-31_down-selected.bag"] - - # Create list of Basic Travel Black Pacifica (Follower) bag files to be processed - BT_follower_black_pacifica_bag_files = ["_2021-07-21-21-13-55_down-selected.bag", - "_2021-07-21-21-30-03_down-selected.bag", - "_2021-07-21-21-37-01_down-selected.bag", - "_2021-07-21-21-49-15_down-selected.bag", - "_2021-07-22-13-09-04_down-selected.bag", - "_2021-07-22-13-31-33_down-selected.bag", - "_2021-07-22-13-41-48_down-selected.bag", - "_2021-07-22-13-52-00_down-selected.bag", - "_2021-07-22-14-03-01_down-selected.bag", - "_2021-07-22-14-10-54_down-selected.bag", - "_2021-07-22-14-22-57_down-selected.bag", - "_2021-07-22-15-08-37_down-selected.bag", - "_2021-07-22-15-20-19_down-selected.bag", - "_2021-07-22-15-31-41_down-selected.bag", - "_2021-07-22-15-42-47_down-selected.bag"] - - # Concatenate all Basic Travel bag files into one list - BT_bag_files = BT_leader_white_pacifica_bag_files + BT_follower_black_pacifica_bag_files - - # Loop to conduct data anlaysis on each bag file: - for bag_file in BT_bag_files: - print("*****************************************************************") - print("Processing new bag: " + str(bag_file)) - if bag_file in BT_leader_white_pacifica_bag_files: - print("Basic Travel Test Case - Leader") - elif bag_file in BT_follower_black_pacifica_bag_files: - print("Basic Travel Test Case - Follower") - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(BT_bag_files.index(bag_file) + 1) + " of " + str(len(BT_bag_files)) + ")") - sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - bag = rosbag.Bag(bag_file_path) - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - # Get the rosbag times associated with entering and exiting the active geofence - print("Getting geofence times at " + str(datetime.datetime.now())) - time_enter_geofence, time_exit_geofence, found_geofence_times = get_geofence_entrance_and_exit_times(bag) - print("Got geofence times at " + str(datetime.datetime.now())) - if (not found_geofence_times): - print("Could not find geofence entrance and exit times in bag file.") - continue - - # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test - print("Getting engagement times at " + str(datetime.datetime.now())) - time_test_start_engagement, time_test_end_engagement, found_test_times = get_test_case_engagement_times(bag, time_enter_geofence, time_exit_geofence) - print("Got engagement times at " + str(datetime.datetime.now())) - if (not found_test_times): - print("Could not find test case engagement start and end times in bag file.") - continue - - # Debug Statements - print("Engagement starts at " + str(time_test_start_engagement.to_sec())) - print("Entered Geofence at " + str(time_enter_geofence.to_sec())) - print("Exited Geofence at " + str(time_exit_geofence.to_sec())) - print("Engagement ends at " + str(time_test_end_engagement.to_sec())) - print("Time spent in geofence: " + str((time_exit_geofence - time_enter_geofence).to_sec()) + " seconds") - print("Time spent engaged: " + str((time_test_end_engagement - time_test_start_engagement).to_sec()) + " seconds") - - original_speed_limit = get_route_original_speed_limit(bag, time_test_start_engagement) # Units: m/s - print("Original Speed Limit is " + str(original_speed_limit) + " m/s") - - # Update the exit geofence time to be based off of /guidance/route_state for improved accuracy - time_exit_geofence = adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit) - print("Adjusted geofence exit time (based on /guidance/route_state): " + str(time_exit_geofence)) - - print_lanelet_entrance_times(bag, time_test_start_engagement) - - downtrack_enter_geofence = get_geofence_entrance_downtrack(bag, time_enter_geofence) - - # Initialize results - b_1_result = None - b_2_result = None - b_3_result = None - b_4_result = None - b_5_result = None - b_6_result = None - b_7_result = None - b_8_result = None - b_9_result = None - b_10_result = None - b_11_result = None - b_12_result = None - b_13_result = None - b_14_result = None - b_15_result = None - b_16_result = None - b_17_result = None - b_18_result = None - b_19_result = None - b_20_result = None - b_21_result = None - b_22_result = None - b_23_result = None - b_24_result = None - b_25_result = None - b_26_result = None - b_27_result = None - - # Metric BT-B-19 - advisory_speed_limit, time_received_first_msg, b_19_result = get_basic_travel_TCM_data(bag) - - # Convert advisory speed limit from BT-B-19 to m/s for future metric evaluations - advisory_speed_limit = advisory_speed_limit * 0.44704 # Conversion from mph to m/s - - lanelets_in_geofence = get_geofence_lanelets(bag, time_enter_geofence, advisory_speed_limit) - - # Metric BT-B-1 - b_1_result = check_basic_travel_geofence_lanelets_in_initial_route(bag, lanelets_in_geofence) - - # Metric BT-B-2 - b_2_result = check_geofence_lanelet_speed_limits(bag, lanelets_in_geofence, advisory_speed_limit) - - # Metric BT-B-3 - b_3_result = check_time_received_first_TCM_message(bag, time_received_first_msg, time_enter_geofence, advisory_speed_limit) - - # Metric BT-B-4: This metric is manually-evaluated using the vehicle speed plot - - # Metric BT-B-5 - b_5_result = None - if bag_file in BT_follower_black_pacifica_bag_files: - check_lane_merge_before_geofence(bag) - else: - print("B-5: N/A (Leader Vehicle)") - - if bag_file in BT_follower_black_pacifica_bag_files: - time_last_sent_follower_join, time_last_sent_join_at_rear, time_received_first_acceptance, time_received_second_acceptance = get_follower_platoon_negotiation_times(bag, time_test_start_engagement) - duration_platoon_negotiation = get_platoon_negotiation_duration(time_received_second_acceptance, time_last_sent_join_at_rear, is_leader=False) - start_lane_following_lanelet = 34915 - b_6_result = check_join_at_rear_negotiation(bag, time_received_first_acceptance, time_last_sent_join_at_rear, time_test_start_engagement, start_lane_following_lanelet) - b_7_result = check_follower_join_negotiation(time_received_first_acceptance, time_last_sent_follower_join) - b_8_result = check_time_headway_after_platoon_negotiation(bag, time_received_second_acceptance) - b_9_result = check_follower_state_after_platoon_negotiation(bag, time_last_sent_join_at_rear) - print("B-10: N/A (Follower Vehicle)") - b_11_result = check_distance_gap_during_platooning(bag, time_received_second_acceptance, time_test_end_engagement) - b_12_result = check_percentage_successful_status_msg_follower(bag, time_received_second_acceptance) - - else: - print("B-6: N/A (Leader Vehicle)") - print("B-7: N/A (Leader Vehicle)") - print("B-8: N/A (Leader Vehicle)") - print("B-9: N/A (Leader Vehicle)") - time_last_received_follower_join, time_last_received_join_at_rear, time_sent_first_acceptance, time_sent_second_acceptance = get_leader_platoon_negotiation_times(bag, time_test_start_engagement) - duration_platoon_negotiation = get_platoon_negotiation_duration(time_sent_second_acceptance, time_last_received_join_at_rear, is_leader=True) - b_10_result = check_leader_state_after_platoon_negotiation(bag, time_last_received_join_at_rear) - print("B-11: N/A (Leader Vehicle)") - b_12_result = check_percentage_successful_status_msg_leader(bag, time_sent_second_acceptance) - b_13_result = check_deceleration_before_geofence(bag, time_enter_geofence, original_speed_limit, advisory_speed_limit) - b_14_result = check_acceleration_distance_after_geofence(bag, time_exit_geofence) - b_15_result = check_acceleration_rate_after_geofence(bag, time_exit_geofence, original_speed_limit, advisory_speed_limit) - - b_16_result = check_steady_state_after_geofence(bag, time_exit_geofence, time_test_end_engagement, original_speed_limit) - - b_17_result = check_speed_limit_when_not_in_geofence(bag, time_test_start_engagement, time_enter_geofence, time_exit_geofence, time_test_end_engagement, original_speed_limit) - - b_18_result = check_advisory_speed_limit(bag, advisory_speed_limit, original_speed_limit) - - if bag_file in BT_follower_black_pacifica_bag_files: - print("B-20: N/A (Follower Vehicle)") - b_21_result = check_vehicle_inside_lane(bag, time_test_start_engagement, time_test_end_engagement, is_front_vehicle = False) - b_22_result = check_vehicle_always_within_maximum_crosstrack(bag, time_test_start_engagement, time_test_end_engagement, is_front_vehicle = False) - print("B-23: N/A (Follower Vehicle)") - print("B-24: N/A (Follower Vehicle)") - b_25_result = check_duration_before_successful_time_headway(bag, time_received_second_acceptance, time_enter_geofence) - print("B-26: N/A (Follower Vehicle)") - print("B-26: N/A (Follower Vehicle)") - else: - b_20_result = check_vehicle_inside_lane(bag, time_test_start_engagement, time_test_end_engagement, is_front_vehicle = True) - print("B-21: N/A (Leader Vehicle)") - print("B-22: N/A (Leader Vehicle)") - b_23_result = check_vehicle_always_within_maximum_crosstrack(bag, time_test_start_engagement, time_test_end_engagement, is_front_vehicle = True) - b_24_result = check_percentage_successful_info_msg(bag, time_sent_second_acceptance) - print("B-25: N/A (Leader Vehicle)") - b_26_result = check_max_time_between_status_msg(bag, time_sent_second_acceptance) - b_27_result = check_max_time_between_info_msg(bag, time_sent_second_acceptance) - - # Get vehicle type that this bag file is from - vehicle_name = "Unknown" - if bag_file in BT_leader_white_pacifica_bag_files: - vehicle_name = "White Pacifica" - elif bag_file in BT_follower_black_pacifica_bag_files: - vehicle_name = "Black Pacifica" - else: - vehicle_name = "N/A" - - # Get test type that this bag file is for - vehicle_role = "Unknown" - if bag_file in BT_leader_white_pacifica_bag_files: - vehicle_role = "Leader" - elif bag_file in BT_follower_black_pacifica_bag_files: - vehicle_role = "Follower" - - # Write simple pass/fail results to .csv file for appropriate row: - csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, - b_1_result, b_2_result, b_3_result, b_4_result, b_5_result, b_6_result, b_7_result, - b_8_result, b_9_result, b_10_result, b_11_result, b_12_result, b_13_result, b_14_result, - b_15_result, b_16_result, b_17_result, b_18_result, b_19_result, b_20_result, - b_21_result, b_22_result, b_23_result, b_24_result, b_25_result, b_26_result, b_27_result]) - - sys.stdout = orig_stdout - text_log_file_writer.close() - return - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_freight_workzone_rosbags.py b/engineering_tools/data_processing/analyze_freight_workzone_rosbags.py deleted file mode 100644 index 5bf66971e9..0000000000 --- a/engineering_tools/data_processing/analyze_freight_workzone_rosbags.py +++ /dev/null @@ -1,1533 +0,0 @@ -#!/usr/bin/python3 - -# 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. - -from inspect import TPFLAGS_IS_ABSTRACT -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime -import math - -# HOW TO USE SCRIPT: -# Run the following in a terminal to download dependencies: -# sudo add-apt-repository ppa:deadsnakes/ppa -# sudo apt-get update -# sudo apt install python3.7 -# python3.7 -m pip install --upgrade pip -# python3.7 -m pip install matplotlib -# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg -# python3.7 -m pip install lz4 -# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ -# In terminal, navigate to the directory that contains this python script and run the following: -# python3.7 analyze_freight_workzone_rosbags.py - - -# Helper function: Creates a plot with 'actual speed vs. time' and 'commanded speed vs. time' -# and saves it as a .png image within the local directory -def generate_speed_plot(bag, time_start_engagement, time_end_engagement, bag_file_name): - # Speed command: /hardware_interface/arbitrated_speed_commands: msg.speed (m/s) - # True Speed: /hardware_interface/vehicle/twist: msg.twist.linear.x (m/s) - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - true_vehicle_speed_times = [] - true_vehicle_speeds = [] - # Note: This topic name assumes a pacmod controller is being used (freightliners or lexus) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement + rospy.Duration(30), end_time = time_start_engagement + rospy.Duration(60)): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - true_vehicle_speed_times.append((t-time_start).to_sec()) - true_vehicle_speeds.append(msg.twist.linear.x) # Current speed in m/s - - # Get the commanded vehicle speed (m/s) and the associated time with each data point - first = True - cmd_vehicle_speed_times = [] - cmd_vehicle_speeds = [] - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/arbitrated_speed_commands'], start_time = time_start_engagement + rospy.Duration(30), end_time = time_start_engagement + rospy.Duration(60)): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - cmd_vehicle_speed_times.append((t-time_start).to_sec()) - cmd_vehicle_speeds.append(msg.speed) # Commanded speed in m/s - - - # Get the time of the first TCM received event - received_tcm_times = [] - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - if msg.tcm_v01.params.detail.choice == 5: - received_tcm_times.append((t-time_start).to_sec()) - break - - # Get the time of all TCRs broadcasted - broadcasted_tcr_times = [] - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_geofence_request'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - broadcasted_tcr_times.append((t-time_start).to_sec()) - - # Get the time of each re-route - route_generation_times = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - route_generation_times.append((t-time_start).to_sec()) - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot commanded vehicle speed (m/s) vs. time - ax.plot(cmd_vehicle_speed_times, cmd_vehicle_speeds, 'g:', label='Commanded Speed (m/s)') - - # Plot true vehicle speed (m/s) vs. time - ax.plot(true_vehicle_speed_times, true_vehicle_speeds, 'b--', label='Actual Speed (m/s)') - - # Optional: Plot a vertical bar at the time of the first received TCM - #ax.axvline(x = received_tcm_times[0], color = 'r', label = 'First TCM Received') - - # Optional: Plot a vertical bar at the time of the first completed re-route - #ax.axvline(x = route_generation_times[1], color = 'orange', label = "Reroute Completed") - - # Optional: Plot a vertical bar for each broadcasted TCR - #for i in range(0, len(broadcasted_tcr_times)): - # if i == 0: - # ax.axvline(x = broadcasted_tcr_times[i], color = 'g', label = 'All TCRs Broadcasted') - # else: - # ax.axvline(x = broadcasted_tcr_times[i], color = 'g') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Speed (Commanded and Actual)") # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Engagement") # Plot X Title - ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title - - # Save the plot - filename = "Speed_" + bag_file_name + ".png" - #plt.savefig(filename, bbox_inches='tight') - - plt.show() # Display the plot - - return - -# Helper function: Creates a plot with 'crosstrack error vs. time' and saves it as a .png image within the local directory -def generate_crosstrack_plot(bag, time_start_engagement, time_end_engagement, bag_file_name): - # Crosstrack Error: /guidance/route_state msg.cross_track (meters) - total_duration = 40 - time_duration = rospy.Duration(total_duration) - - # Get the cross track error and the time associated with each data point - first = True - crosstrack_errors = [] - crosstrack_error_times = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement + rospy.Duration(0.0), end_time = time_end_engagement - rospy.Duration(0.0)): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - crosstrack_error_times.append((t-time_start).to_sec()) - crosstrack_errors.append(msg.cross_track) # Crosstrack Error (meters) - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot true cross-track error (meters) vs. time - ax.plot(crosstrack_error_times, crosstrack_errors, 'g', label='Cross-track Error (Meters)') - - # Optional: Plot a horizontal bar at a positive value for reference - ax.axhline(y = 0.65, color = 'r', label = '+/- 0.65 Meters (For Reference)') - - # Optional: Plot a horizontal bar at a negative value for reference - ax.axhline(y = -0.65, color = 'r') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Cross-track Error") # Figure Title - ax.set_xlabel("Time (seconds) Since Start of Engagement") # Figure X Label - ax.set_ylabel("Cross-track Error (meters)") # Figure Y Label - - # Save the plot - filename = "CrossTrack_" + bag_file_name + ".png" - plt.savefig(filename, bbox_inches='tight') - - #plt.show() # Display the plot - - return - -# Helper function: Creates a plot with 'actual steering angle vs. time' and 'commanded steering angle vs. time' -# and saves it as a .png image within the local directory -def generate_steering_plot(bag, engagement_times, route_id): - # Speed command: /hardware_interface/arbitrated_speed_commands: msg.speed - # True Speed: /hardware_interface/pacmod_parsed_tx/vehicle_speed_rpt: msg.vehicle_speed - total_duration = 25 - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - time_duration = rospy.Duration(total_duration) - - # Get the true vehicle speed and plot it - first = True - true_steering_times = [] - true_steering = [] - cmd_steering_times = [] - cmd_steering = [] - # Note: This topic name assumes a pacmod controller is being used (freightliners or lexus) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/pacmod/parsed_tx/steer_rpt'], start_time = time_start_engagement, end_time = time_end_engagement): #time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - true_steering_times.append((t-time_start).to_sec()) - true_steering.append(msg.output) - cmd_steering_times.append((t-time_start).to_sec()) - cmd_steering.append(msg.command) - - - fig, ax = plt.subplots() - ax.plot(true_steering_times, true_steering, 'b--', label='Actual Steering (rad)') - ax.plot(cmd_steering_times, cmd_steering, 'g:', label='Cmd Steering (rad)') - ax.legend() - ax.set_title("Steering Angle (Cmd and Actual) for Route " + str(route_id)) - ax.set_xlabel("Time (seconds) since start of engagement") - ax.set_ylabel("Steering Angle (rad)") - plt.show() - - return - -# Helper Function: Get times associated with the system entering the geofence and exiting the geofence -def get_geofence_entrance_and_exit_times(bag): - # Initialize geofence entrance and exit times - time_enter_active_geofence = rospy.Time() # Time that the vehicle has entered the geofence - time_exit_active_geofence = rospy.Time() # Time that the vehicle has exited the geofence-------------------------------------------------------------------------------------------------------- - - # Find geofence entrance and exit times - is_on_active_geofence = False - found_geofence_entrance_time = False - found_geofence_exit_time = False - for topic, msg, t in bag.read_messages(topics=['/environment/active_geofence']): - # If first occurrence of being in the active geofence, set the start time - if (msg.is_on_active_geofence and not is_on_active_geofence): - time_enter_active_geofence = t - #print("Entered geofence at " + str(t.to_sec())) - #print(msg) - found_geofence_entrance_time = True - is_on_active_geofence = True - - # If final occurrence of being in the active geofence, set the end time - if (not msg.is_on_active_geofence and is_on_active_geofence): - time_exit_active_geofence = t - found_geofence_exit_time = True - - time_in_geofence = (time_exit_active_geofence - time_enter_active_geofence).to_sec() - print("Spent " + str(time_in_geofence) + " sec in geofence. Started at " + str(time_enter_active_geofence.to_sec())) - is_on_active_geofence = False - #break - - # Check if both geofence start and end time were found - found_geofence_times = False - if (found_geofence_entrance_time and found_geofence_exit_time): - found_geofence_times = True - - return time_enter_active_geofence, time_exit_active_geofence, found_geofence_times - -# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section -def get_test_case_engagement_times(bag, time_enter_active_geofence, time_exit_active_geofence): - # Initialize system engagement start and end times - time_start_engagement = rospy.Time() - time_stop_engagement = rospy.Time() - - # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section - is_engaged = False - found_engagement_times = False - has_reached_geofence_entrance = False - has_reached_geofence_exit = False - for topic, msg, t in bag.read_messages(topics=['/guidance/state']): - # If entering engagement, track this start time - if (msg.state == 4 and not is_engaged): - time_start_engagement = t - is_engaged = True - - # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance - # state can be published. - if (msg.state == 4): - time_last_engaged = t - - # If exiting engagement, check whether this period of engagement included the geofence entrance and exit times - elif (msg.state != 4 and is_engaged): - is_engaged = False - time_stop_engagement = t - - # Check if the engagement start time was before the geofence entrance and exit times - if (time_start_engagement <= time_enter_active_geofence and t >= time_enter_active_geofence): - has_reached_geofence_entrance = True - if (time_start_engagement <= time_exit_active_geofence and t >= time_exit_active_geofence): - has_reached_geofence_exit = True - - # Set flag if this engagement period includes both the geofence entrance and exit times - if (has_reached_geofence_entrance and has_reached_geofence_exit): - found_test_case_engagement_times = True - break - - # If CARMA ended engagement before guidance state could be updated, check if the last recorded - # time of engagement came after exiting the geofence - if not found_engagement_times: - if time_last_engaged >= time_exit_active_geofence: - time_stop_engagement = time_last_engaged - found_engagement_times = True - - return time_start_engagement, time_stop_engagement, found_engagement_times - -# Helper Function: Get the original speed limit for the lanelets within the vehicle's route -# Note: Assumes that all lanelets in the route share the same speed limit prior to the first geofence CARMA Cloud message being processed. -def get_route_original_speed_limit(bag, time_test_start_engagement): - # Initialize the return variable - original_speed_limit = 0.0 - - # Find the speed limit associated with the first lanelet when the system first becomes engaged - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_test_start_engagement): - original_speed_limit = msg.speed_limit - break - - return original_speed_limit - - -########################################################################################################### -# Workzone FWZ-1: The geofenced area is a part of the initial route plan. -# -# Workzone FWZ-8: The vehicle receives a message from CC that includes the closed lane ahead. The vehicle -# processes this closed lane information. -########################################################################################################### -def check_geofence_in_initial_route(bag, closed_lanelets): - # Get each set route from the bag file (includes set routes and updated/re-rerouted routes) - shortest_path_lanelets = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route']): - # Print as Debug Statement - print("Shortest Path Route Update at " + str(t.to_sec()) + ": " + str(msg.shortest_path_lanelet_ids)) - - shortest_path_lanelets.append([]) - for lanelet_id in msg.shortest_path_lanelet_ids: - shortest_path_lanelets[-1].append(lanelet_id) - - # If there are two route paths, check that the first (original) route contains the closed lanelet(s) and the second route doesn't - # Note: Assumes there should be only two routes: (1) the initial route and (2) the re-routed route - initial_route_includes_closed_lane = False # Flag for B-1 success - map_is_updated_for_closed_lane = False # Flag for B-11 success - if (len(shortest_path_lanelets) > 1): - original_shortest_path = shortest_path_lanelets[0] - rerouted_shortest_path = shortest_path_lanelets[-1] - - for lanelet_id in closed_lanelets: - if lanelet_id in original_shortest_path: - initial_route_includes_closed_lane = True - else: - initial_route_includes_closed_lane = False - break - - for lanelet_id in closed_lanelets: - if lanelet_id not in rerouted_shortest_path: - map_is_updated_for_closed_lane = True - else: - map_is_updated_for_closed_lane = False - break - else: - print("Invalid quantity of route updates found in bag file (" + str(len(shortest_path_lanelets)) + " found, more than 1 expected)") - - # Print result statements and return success flags - if (initial_route_includes_closed_lane): - print("FWZ-1 succeeded; all closed lanelets " + str(closed_lanelets) + " were in the initial route") - else: - print("FWZ-1 failed: not all closed lanelets " + str(closed_lanelets) + " were in the initial route.") - - if (map_is_updated_for_closed_lane): - print("FWZ-8 succeeded: no closed lanelets " + str(closed_lanelets) + " were in the re-routed route.") - else: - print("FWZ-8 failed: at least 1 closed lanelet " + str(closed_lanelets) + " was in the re-routed route.") - - return initial_route_includes_closed_lane, map_is_updated_for_closed_lane - - -########################################################################################################### -# FWZ-2: The vehicle will travel at steady-state (e.g. same lane, constant speed) for at least 5 seconds -# before it enters the geofenced area defined in the CC message. -########################################################################################################### -def check_start_steady_state_before_geofence(bag, time_start_engagement, time_enter_geofence, original_speed_limit): - # (m/s) Threshold offset of vehicle speed to speed limit to be considered at steady state - threshold_speed_limit_offset = 0.89408 # 0.89408 m/s is 2 mph - min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset - max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset - - # (seconds) Minimum time between vehicle reaching steady state and and vehicle entering geofence - min_time_between_steady_state_and_geofence = 5.0 - - # Get the time that the vehicle reaches within the set offset of the speed limit (while system is engaged) - time_start_steady_state = 0.0 - has_reached_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement): - current_speed = msg.twist.linear.x # Current vehicle speed in m/s - if (max_steady_state_speed >= current_speed >= min_steady_state_speed): - has_reached_steady_state = True - time_start_steady_state = t - break - - # Check if the time the vehicle reaches steady state is more than 'min_time_between_steady_state_and_geofence' seconds before entering the geofence - if (has_reached_steady_state): - time_between_steady_state_and_geofence = (time_enter_geofence - time_start_steady_state).to_sec() - if (time_between_steady_state_and_geofence >= min_time_between_steady_state_and_geofence): - is_successful = True - print("FWZ-2 succeeded; reached steady state " + str(time_between_steady_state_and_geofence) + " seconds before entering geofence.") - else: - is_successful = False - if (time_between_steady_state_and_geofence > 0): - print("FWZ-2 failed; reached steady state " + str(time_between_steady_state_and_geofence) + " seconds before entering geofence.") - else: - print("FWZ-2 failed; reached steady state " + str(-time_between_steady_state_and_geofence) + " seconds after entering geofence.") - else: - print("FWZ-2 failed; vehicle never reached steady state during rosbag recording.") - is_successful = False - - return is_successful, time_start_steady_state - -########################################################################################################### -# FWZ-7: The vehicle receives a message from CS that includes the new speed limit for the geofence area. -# The vehicle successfully processes this new speed limit. -########################################################################################################### -def check_in_geofence_speed_limits(bag, time_enter_geofence, time_exit_geofence, advisory_speed_limit): - ms_threshold = 0.03 # Threshold that a received/processed speed limit can differ from the expected advisory speed limit - - # Check that a TrafficControlMessage was published using the correct advisory speed limit - has_communicated_advisory_speed_limit = False - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - if (msg.tcm_v01.params.detail.choice == 12 and advisory_speed_limit - ms_threshold <= msg.tcm_v01.params.detail.maxspeed <= advisory_speed_limit + ms_threshold): - has_communicated_advisory_speed_limit = True - break - - # Check that lanelets travelled through within the geofence have the expected advisory speed limit applied - time_buffer = rospy.Duration(2.0) # (Seconds) Buffer after entering geofence and before exiting geofence for which advisory speed limit is observed - has_correct_geofence_lanelet_speed_limits = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = (time_enter_geofence+time_buffer), end_time = (time_exit_geofence-time_buffer)): - # Failure if the current lanelet is one of the geofence lanelets and its speed limit doesn't match the advisory speed limit - if (abs(msg.speed_limit - advisory_speed_limit) >= ms_threshold): - print("Lanelet ID " + str(msg.lanelet_id) + " has speed limit of " + str(msg.speed_limit) + " m/s. " + \ - "Does not match advisory speed limit of " + str(advisory_speed_limit) + " m/s.") - has_correct_geofence_lanelet_speed_limits = False - break - - if (has_communicated_advisory_speed_limit and has_correct_geofence_lanelet_speed_limits): - print("FWZ-7 succeeded; system received and processed an advisory speed limit of " + str(advisory_speed_limit) + " m/s") - is_successful = True - else: - print("FWZ-7 failed; system did not receive and process an advisory speed limit of " + str(advisory_speed_limit) + " m/s") - is_successful = False - - return is_successful - -########################################################################################################### -# FWZ-9: The vehicle receives a message from CS that includes info about the restricted lane ahead. -# The vehicle successfully processes this restricted lane information. -########################################################################################################### -def check_restricted_lane_tcm_received(bag, time_start_engagement, time_end_engagement, restricted_lanelets): - # Check that a TrafficControlMessage was published for a closed lane that is not closed to passenger vehicles - has_communicated_restricted_lane = False - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - # Evaluate a received TCM for a closed lane - if (msg.tcm_v01.params.detail.choice == 5): - - # Determine whether the closed lane is closed to passenger vehicles - # Note: Lane is considered restricted if it is not closed to passenger vehicles - is_restricted_lane = True - for value in msg.tcm_v01.params.vclasses: - # vehicle_class 5 is passenger vehicles - if value.vehicle_class == 5: - is_restricted_lane = False - - # Set boolean flags for this metric - if is_restricted_lane: - has_communicated_restricted_lane = True - break - - # Get each set route from the bag file (includes set routes and updated/re-rerouted routes) - shortest_path_lanelets = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route']): - shortest_path_lanelets.append([]) - for lanelet_id in msg.shortest_path_lanelet_ids: - shortest_path_lanelets[-1].append(lanelet_id) - - # Verify that the final route update does not include the restricted lanelets - map_is_updated_for_restricted_lane = False # Flag for B-11 success - if (len(shortest_path_lanelets) > 1): - rerouted_shortest_path = shortest_path_lanelets[-1] - - - for lanelet_id in restricted_lanelets: - if lanelet_id not in rerouted_shortest_path: - map_is_updated_for_restricted_lane = True - else: - map_is_updated_for_restricted_lane = False - break - else: - print("Invalid quantity of route updates found in bag file (" + str(len(shortest_path_lanelets)) + " found, more than 1 expected)") - - # Print result statements and return success flags - is_successful = False - if (has_communicated_restricted_lane and map_is_updated_for_restricted_lane): - print("FWZ-9 succeeded: received restricted lane TCM and the restricted lanelets " + str(restricted_lanelets) + " were not in the re-routed route.") - is_successful = True - elif (has_communicated_advisory_speed_limit and not map_is_updated_for_restricted_lane): - print("FWZ-9 failed: received restricted lane TCM but the restricted lanelets " + str(restricted_lanelets) + " were in the re-routed route.") - else: - print("FWZ-9 failed: CMV did not received restricted lane TCM.") - - return is_successful - -########################################################################################################### -# FWZ-10: The vehicle returns an Acknowledgement message within 1 second after successfully processing -# a TCM from CS.F -########################################################################################################### -def check_tcm_acknowledgements(bag, time_start_engagement, time_end_engagement, num_tcms): - max_duration_before_ack = 1.0 # (seconds) Maximum duration between receiving a TCM and acknowledging it - - # Initialize array of times that each TCM is received - # Note: Index 0 is TCM msgnum 1, Index 1 is TCM msgnum 2, etc. - tcm_receive_times = [] - for i in range(0, num_tcms): - tcm_receive_times.append(None) - - # Initialize array of durations between receiving a TCM and broadcasting an acknowledgement - # Note: Index 0 is TCM msgnum 1, Index 1 is TCM msgnum 2, etc. - tcm_ack_durations = [] - for i in range(0, num_tcms): - tcm_ack_durations.append(None) - - # Get the time that each TCM was received - first = True - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - if first: - t_first_tcm = t - first = False - - msg_num = msg.tcm_v01.msgnum - #print("Received msg_num: " + str(msg_num)) - if tcm_receive_times[msg_num - 1] == None: - tcm_receive_times[msg_num - 1] = t - if None not in tcm_receive_times: - print("FWZ-10 (DEBUG): Received all TCMs") - break - - # Check the first positive acknowledgement time associated with each received TCM - num = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation']): - if msg.strategy == "carma3/Geofence_Acknowledgement": - if "acknowledgement:1" in msg.strategy_params: - msg_split = msg.strategy_params.split(':') # Split string by ':' character - msg_num = int(msg_split[2].split(',')[0]) # Split the second elemnt of the above split by the ',' character and obtain the first element - if tcm_ack_durations[msg_num - 1] == None: - print(msg.strategy_params) - print("FWZ-10 (DEBUG): Acknowledging TCM msgnum " + str(msg_num) + " " + str((t - t_first_tcm).to_sec()) + " sec after receiving first TCM") - duration = (t - tcm_receive_times[msg_num - 1]).to_sec() - tcm_ack_durations[msg_num - 1] = duration - if None not in tcm_ack_durations: - print("FWZ-10 (DEBUG): Acknowledged all TCMs") - break - - is_successful = True - for i in range(0, num_tcms): - if tcm_ack_durations[i] <= max_duration_before_ack: - print("FWZ-10 (msgnum " + str(i+1) + ") successful; TCM Ack broadcasted in " + str(tcm_ack_durations[i]) + " seconds") - else: - is_successful = False - print("FWZ-10 (msgnum " + str(i+1) + ") failed; TCM Ack broadcasted in " + str(tcm_ack_durations[i]) + " seconds") - - return is_successful - -########################################################################################################### -# FWZ-12: After the vehicle has received a message from CS with the Work Zone information, the vehicle -# shall successfully update its active route in less than 3 seconds to avoid the Work Zone. -########################################################################################################### -def check_reroute_duration(bag, time_start_engagement, time_end_engagement): - # Note: Implementation assumes 1 closed lane TCM and 1 restricted lane TCM are received - - max_duration_before_reroute = 3.0 # (seconds) Maximum duration between receiving a closed/restricted lane TCM and rerouting - - # Obtain timestamps of each closed/restricted lane TCM - closed_lane_tcm_receive_time = None - restricted_lane_tcm_receive_time = None - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - # Evaluate a received TCM for a closed lane - if (msg.tcm_v01.params.detail.choice == 5): - - # Determine whether the closed lane is closed to passenger vehicles - # Note: Lane is considered restricted if it is not closed to passenger vehicles - is_restricted_lane = True - for value in msg.tcm_v01.params.vclasses: - # vehicle_class 5 is passenger vehicles - if value.vehicle_class == 5: - is_restricted_lane = False - - # Set boolean flags for this metric - if is_restricted_lane: - if restricted_lane_tcm_receive_time is None: - restricted_lane_tcm_receive_time = t - print("FWZ-12 (DEBUG): Received restricted lane TCM at " + str(t)) - else: - if closed_lane_tcm_receive_time is None: - closed_lane_tcm_receive_time = t - print("FWZ-12 (DEBUG): Received closed lane TCM at " + str(t)) - - if closed_lane_tcm_receive_time <= restricted_lane_tcm_receive_time: - closed_lane_tcm_received_first = True - else: - closed_lane_tcm_received_first = False - - # Get the time of each re-route - route_generation_times = [] - first = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route']): - print("FWZ-12 (DEBUG): Generated route at " + str(t)) - route_generation_times.append(t) - - if closed_lane_tcm_received_first: - duration_reroute_after_closed_lane_tcm_received = (route_generation_times[1] - closed_lane_tcm_receive_time).to_sec() - duration_reroute_after_restricted_lane_tcm_received = (route_generation_times[2] - restricted_lane_tcm_receive_time).to_sec() - else: - duration_reroute_after_closed_lane_tcm_received = (route_generation_times[2] - closed_lane_tcm_receive_time).to_sec() - duration_reroute_after_restricted_lane_tcm_received = (route_generation_times[1] - restricted_lane_tcm_receive_time).to_sec() - - is_successful = True - if duration_reroute_after_closed_lane_tcm_received <= max_duration_before_reroute: - print("FWZ-12 succeeded; rerouted " + str(duration_reroute_after_closed_lane_tcm_received) + " sec after receiving closed lane TCM") - else: - print("FWZ-12 failed; rerouted " + str(duration_reroute_after_closed_lane_tcm_received) + " sec after receiving closed lane TCM") - is_successful = False - - if duration_reroute_after_restricted_lane_tcm_received <= max_duration_before_reroute: - print("FWZ-12 succeeded; rerouted " + str(duration_reroute_after_restricted_lane_tcm_received) + " sec after receiving restricted lane TCM") - else: - print("FWZ-12 failed; rerouted " + str(duration_reroute_after_restricted_lane_tcm_received) + " sec after receiving restricted lane TCM") - is_successful = False - - return is_successful - -########################################################################################################### -# FWZ-14: The vehicle lateral velocity during a lane change remains between 0.5 m/s and 1.25 m/s. -########################################################################################################### -def check_lanechange_lateral_acceleration(bag, time_start_engagement, time_end_engagement, num_expected_lanechanges): - return - -########################################################################################################### -# FWZ-15: After changing lanes, the vehicle will achieve steady-state (i.e. truck is driving within the lane) -# within 5 seconds. -########################################################################################################### -def check_lanechange_duration(bag, time_start_engagement, time_end_engagement, num_expected_lanechanges): - max_lanechange_duration = 5.0 - crosstrack_end_lanechange = 3.2 # (meters) Distance from original lane center for lane change to be considered complete - - # Get each timestamp of the first trajectory plan point being planned by CLC - first = True - is_in_lanechange = False - num = 0 - times_start_lanechange = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/plan_trajectory'], start_time = time_start_engagement + rospy.Duration(45.0)): - if msg.trajectory_points[0].planner_plugin_name == "StopAndWaitPlugin": - break - - if not is_in_lanechange: - if msg.trajectory_points[0].planner_plugin_name != "InLaneCruisingPlugin": - is_in_lanechange = True - times_start_lanechange.append(t) - - if is_in_lanechange: - if msg.trajectory_points[0].planner_plugin_name == "InLaneCruisingPlugin": - is_in_lanechange = False - - print("FWZ-15 (DEBUG): Found starting time of " + str(len(times_start_lanechange)) + " lane changes") - lanechange_durations = [] - lanechange_lateral_movement = [] - for time in times_start_lanechange: - first = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time): - if first: - lanechange_lateral_movement.append(crosstrack_end_lanechange - abs(msg.cross_track)) - first = False - if abs(msg.cross_track) >= crosstrack_end_lanechange: - lanechange_durations.append((t-time).to_sec()) - break - print("FWZ-15 (DEBUG): Starting cross tracks: " + str(lanechange_lateral_movement)) - is_successful = True - for i in range(0, len(lanechange_durations)): - if lanechange_durations[i] <= max_lanechange_duration: - print("FWZ-15 (LC " + str(i + 1) + ") succeeded; lane change completed in " + str(lanechange_durations[i]) + " seconds") - else: - print("FWZ-15 (LC " + str(i + 1) + ") failed; lane change completed in " + str(lanechange_durations[i]) + " seconds") - is_successful = False - - return is_successful - -########################################################################################################### -# FWZ-23: Upon entering the geofenced area with an advisory speed limit, the vehicle will initiate the -# deceleration command to the advisory speed limit within less than 1.3 seconds. -########################################################################################################### -def check_time_to_begin_deceleration(bag, time_start_engagement, time_end_engagement, advisory_speed_limit_ms): - max_duration_before_decel = 1.3 # seconds - - speed_limit_threshold_ms = 0.05 - min_speed_limit_ms = advisory_speed_limit_ms - speed_limit_threshold_ms - max_speed_limit_ms = advisory_speed_limit_ms + speed_limit_threshold_ms - time_enter_geofence = None - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_end_engagement): - # Obtain the time that the vehicle enters a lanelet with the advisory speed limit - if (min_speed_limit_ms <= msg.speed_limit <= max_speed_limit_ms): - time_enter_geofence = t - break - - num_consecutive_decel_required = 10 # Arbitrarily select that topic must show vehicle slowing down for this many consecutive messages to be considered the start of deceleration - found_start_of_decel = False - duration_before_decel = rospy.Duration(0.0) - time_begin_deceleration_in_geofence = rospy.Time(0.0) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_enter_geofence, end_time = time_end_engagement): # time_start_engagement+time_duration): - time_start = t - - num_consecutive_decel = 0 - first = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - prev_speed = msg.twist.linear.x - first = False - continue - - speed_start_decel_ms = msg.twist.linear.x - if msg.twist.linear.x - prev_speed < 0: - num_consecutive_decel += 1 - if num_consecutive_decel == num_consecutive_decel_required: - duration_before_decel = (time_start - time_enter_geofence).to_sec() - time_begin_deceleration_in_geofence = t - found_start_of_decel = True - else: - break - - prev_speed = msg.twist.linear.x - - if found_start_of_decel: - break - - is_successful = False - if duration_before_decel < max_duration_before_decel: - print("FWZ-23 succeeded; vehicle began decelerating " + str(duration_before_decel) + " seconds after entering geofence") - is_successful = True - else: - print("FWZ-23 failed; vehicle began decelerating " + str(duration_before_decel) + " seconds after entering geofence") - - - return is_successful, time_begin_deceleration_in_geofence - -########################################################################################################### -# FWZ-24: The vehicle will decelerate and achieve the advisory speed limit prior to reaching the work zone. -########################################################################################################### -def check_speed_before_workzone(bag, time_start_engagement, time_end_engagement, workzone_lanelet_id, advisory_speed_limit_ms): - speed_limit_threshold_ms = 0.89408 # 0.89408 m/s is 2 mph - min_speed_limit_ms = advisory_speed_limit_ms - speed_limit_threshold_ms - max_speed_limit_ms = advisory_speed_limit_ms + speed_limit_threshold_ms - - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_end_engagement): - if msg.lanelet_id == workzone_lanelet_id: - time_enter_workzone = t - - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_enter_workzone, end_time = time_end_engagement): # time_start_engagement+time_duration): - vehicle_speed_workzone_entrance_ms = msg.twist.linear.x - break - - is_successful = False - if (min_speed_limit_ms <= vehicle_speed_workzone_entrance_ms <= max_speed_limit_ms): - print("FWZ-24 succeeded: Vehicle travelling at " + str(vehicle_speed_workzone_entrance_ms) + " m/s when entering the workzone") - is_successful = True - else: - print("FWZ-24 failed: Vehicle travelling at " + str(vehicle_speed_workzone_entrance_ms) + " m/s when entering the workzone") - - return is_successful - -########################################################################################################### -# FWZ-25: Upon entering the geofenced area with an advisory speed limit, the actual trajectory to the reduced -# speed operations will include an acceleration section. The average deceleration over the entire -# section shall be no greater than 2 m/s^2. -########################################################################################################### -def check_deceleration_for_geofence(bag, time_start_engagement, time_end_engagement, time_begin_deceleration_in_geofence, advisory_speed_limit_ms): - max_avg_deceleration = 2.0 # m/s^2 - end_decel_speed = advisory_speed_limit_ms + 1.1176 # Add 1.1176 m/s (2.5 mph) buffer to the expected end-of-deceleration speed - - # Obtain timestamp associated with the end of the deceleration section - # Note: This is the moment when the vehicle's speed reaches the advisory speed limit - first = True - time_end_decel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_begin_deceleration_in_geofence): - # Obtain the speed at the start of the deceleration section - if first: - speed_start_decel_ms = msg.twist.linear.x - first = False - - # Print Debug Line - speed_start_decel_mph = msg.twist.linear.x * 2.2639 # 2.2369 mph is 1 m/s - print("FWZ-25 (DEBUG): Speed at start of deceleration section: " + str(speed_start_decel_mph) + " mph (" + str(speed_start_decel_ms) + " m/s)") - continue - - current_speed_ms = msg.twist.linear.x - if (current_speed_ms <= end_decel_speed): - time_end_decel = t - speed_end_decel_ms = current_speed_ms - - # Print Debug Line - speed_end_decel_mph = speed_end_decel_ms * 2.2369 # 2.2369 mph is 1 m/s - print("FWZ-25 (DEBUG): Speed at end of deceleration section: " + str(speed_end_decel_mph) + " mph (" + str(speed_end_decel_ms) + " m/s)") - break - - - # Calculate the average deceleration across the full deceleration section - print("FWZ-25 (DEBUG): Duration between start and end of deceleration: " + str((time_end_decel-time_begin_deceleration_in_geofence).to_sec())) - total_average_decel = (speed_start_decel_ms - speed_end_decel_ms) / (time_end_decel - time_begin_deceleration_in_geofence).to_sec() - - is_successful = False - if total_average_decel <= max_avg_deceleration: - print("FWZ-25 succeeded: average deceleration after entering geofence was " + str(total_average_decel) + " m/s^2") - is_successful = True - else: - print("FWZ-25 succeeded: average deceleration after entering geofence was " + str(total_average_decel) + " m/s^2") - - return is_successful - - -########################################################################################################### -# FWZ-26: After exiting the geofenced area with an advisory speed limit, the vehicle will begin accelerating -# back to the original speed limit within less than 1.3 seconds. -########################################################################################################### -def check_time_to_begin_acceleration(bag, time_enter_geofence, time_end_engagement, original_speed_limit_ms): - max_duration_before_accel = 10.0 # seconds - - # Obtain the time that the CMV enters a lanelet with the original speed limit (this indicates it has exited the geofenced area) - speed_limit_threshold_ms = 0.05 - min_speed_limit_ms = original_speed_limit_ms - speed_limit_threshold_ms - max_speed_limit_ms = original_speed_limit_ms + speed_limit_threshold_ms - time_exit_geofence = None - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_enter_geofence, end_time = time_end_engagement): - # Obtain the time that the vehicle enters a lanelet with the advisory speed limit - if (min_speed_limit_ms <= msg.speed_limit <= max_speed_limit_ms): - time_exit_geofence = t - break - - num_consecutive_accel_required = 20 # Arbitrarily select that topic must show vehicle speeding up for this many consecutive messages to be considered the start of acceleration - found_start_of_accel = False - duration_before_accel = rospy.Duration(0.0) - time_begin_acceleration_after_geofence = rospy.Time(0.0) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_exit_geofence, end_time = time_end_engagement): # time_start_engagement+time_duration): - time_start = t - - num_consecutive_accel = 0 - first = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - prev_speed = msg.twist.linear.x - first = False - continue - - if msg.twist.linear.x - prev_speed > 0: - num_consecutive_accel +=1 - if num_consecutive_accel == num_consecutive_accel_required: - duration_before_accel = (time_start - time_exit_geofence).to_sec() - time_begin_acceleration_after_geofence = time_start - found_start_of_accel = True - else: - break - - prev_speed = msg.twist.linear.x - - if found_start_of_accel: - break - - - is_successful = False - if duration_before_accel <= max_duration_before_accel: - print("FWZ-26 succeeded; vehicle began accelerating " + str(duration_before_accel) + " seconds after exiting geofence") - is_successful = True - else: - print("FWZ-26 failed; vehicle began accelerating " + str(duration_before_accel) + " seconds after exiting geofence") - - return is_successful, time_begin_acceleration_after_geofence - -########################################################################################################### -# FWZ-27: After exiting the geofenced area with an advisory speed limit, the actual trajectory back to -# normal operations will include an acceleration section. The average acceleration over the entire -# section shall be no less than 1 m/s^2, and the average acceleration over any 1-second portion -# of the section shall be no greater than 2.0 m/s^2. -########################################################################################################### -def check_acceleration_after_geofence(bag, time_begin_acceleration_after_geofence, time_end_engagement, target_speed_limit_ms): - min_average_accel_rate = 0.0 # m/s^2 - max_one_second_accel_rate = 1.5 # m/s^2 - - # Set target speed (an offset below the target speed limit, since acceleration rate will decrease after this point) - speed_limit_offset_ms = 0.89 # 0.89 m/s is 2 mph - target_speed_ms = target_speed_limit_ms - speed_limit_offset_ms - - # Obtain timestamp associated with the end of the acceleration section - # Note: This is either the first deceleration after the start of the accel section or the moment - # when the vehicle's speed reaches some offset of the speed limit. - prev_speed = 0.0 - first = True - time_end_accel = rospy.Time() - speed_start_accel_ms = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_begin_acceleration_after_geofence): - if first: - prev_speed = msg.twist.linear.x - speed_start_accel_ms = msg.twist.linear.x - first = False - continue - - delta_speed = msg.twist.linear.x - prev_speed - current_speed_ms = msg.twist.linear.x - if (current_speed_ms >= target_speed_ms): - time_end_accel = t - speed_end_accel_ms = current_speed_ms - - # Debug Line - speed_end_accel_mph = speed_end_accel_ms * 2.2369 - print("FWZ-27 (DEBUG): Speed at end of acceleration section: " + str(speed_end_accel_mph) + " mph") - break - - prev_speed = msg.twist.linear.x - - # Check the total average acceleration rate - total_avg_accel = (speed_end_accel_ms - speed_start_accel_ms) / (time_end_accel-time_begin_acceleration_after_geofence).to_sec() - #print("Total average accel: " + str(total_avg_accel) + " m/s^2") - - # Check the 1-second window averages - # Obtain average decelation over all 1-second windows in acceleration section - one_second_duration = rospy.Duration(1.0) - all_one_second_windows_successful = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_begin_acceleration_after_geofence, end_time = (time_end_accel-one_second_duration)): - speed_initial_ms = msg.twist.linear.x - time_initial = t - - speed_final_ms = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = (time_initial+one_second_duration)): - speed_final_ms = msg.twist.linear.x - t_final = t - break - - one_second_accel = (speed_final_ms - speed_initial_ms) / (t_final - time_initial).to_sec() - - if one_second_accel > max_one_second_accel_rate: - print("Failure: 1-second window accel rate was " + str(one_second_accel) + " m/s^2; end time was " + str((t_final - t_final).to_sec()) + " seconds after start of engagement.") - all_one_second_windows_successful = False - #else: - # print("Success: 1-second window has accel rate at " + str(one_second_decel) + " m/s^2") - - # Print success/failure statements - total_acceleration_rate_successful = False - if total_avg_accel >= min_average_accel_rate: - print("FWZ-27 (avg accel after geofence) Succeeded; total average acceleration was " + str(total_avg_accel) + " m/s^2") - total_acceleration_rate_successful = True - else: - print("FWZ-27 (avg accel at start) Failed; total average acceleration was " + str(total_avg_accel) + " m/s^2") - - if all_one_second_windows_successful: - print("FWZ-27 (1-second accel windows) Succeeded; no occurrences of 1-second average acceleration above 1.5 m/s^2") - else: - print("FWZ-27 (1-second accel windows) Failed; at least one occurrence of 1-second average acceleration above 1.5 m/s^2") - - is_successful = False - if total_acceleration_rate_successful and all_one_second_windows_successful: - is_successful = True - - return is_successful - -########################################################################################################### -# FWZ-29: After exiting the geofenced area, the planned route must end with the vehicle having been at -# steady state for at least 5 seconds.  -########################################################################################################### -def check_steady_state_after_geofence(bag, time_begin_acceleration_after_geofence, time_end_engagement, original_speed_limit_ms): - min_time_at_steady_state = 5.0 # Seconds - - # (m/s) Threshold offset of vehicle speed to speed limit to be considered at steady state - threshold_speed_limit_offset = 0.89408 # 0.89408 m/s is 2 mph - min_steady_state_speed = original_speed_limit_ms - threshold_speed_limit_offset - max_steady_state_speed = original_speed_limit_ms + threshold_speed_limit_offset - - has_reached_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_begin_acceleration_after_geofence, end_time = time_end_engagement): - if (min_steady_state_speed <= msg.twist.linear.x <= max_steady_state_speed) and not has_reached_steady_state: - time_start_steady_state = t - has_reached_steady_state = True - - if not (min_steady_state_speed <= msg.twist.linear.x <= max_steady_state_speed) and has_reached_steady_state: - time_end_steady_state = t - break - elif has_reached_steady_state: - time_end_steady_state = t - - if has_reached_steady_state: - time_at_steady_state = (time_end_steady_state - time_start_steady_state).to_sec() - else: - time_at_steady_state = 0.0 - - is_successful = False - if time_at_steady_state >= min_time_at_steady_state: - print("FWZ-29 succeeded: Vehicle was at steady state for " + str(time_at_steady_state) + " seconds after exiting the geofence") - is_successful = True - else: - print("FWZ-29 failed: Vehicle was at steady state for " + str(time_at_steady_state) + " seconds after exiting the geofence") - - return is_successful - -########################################################################################################### -# FWZ-32: The time taken between when a TCR is sent from the vehicle to a TCM is received by -# the vehicle shall be less than 1 second. -########################################################################################################### -def check_tcm_response_time(bag, time_enter_geofence, tcr_rx_times, tcr_rx_reqids, reqid_v2x_timestamps): - # reqid_v2x_timestamps: 0 is reqid; 1 is tcr receive time; 2-10 are FIRST tcm tx times for msgnums 0 to 9 - tcr_reqids = [] - tcr_reqids_times = [] - num = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_geofence_request']): - #print(str(num) + ": " + str(msg.tcr_v01.reqid)) - tcr_reqids.append([int(i) for i in msg.tcr_v01.reqid.id]) - hex_string = "" - for i in msg.tcr_v01.reqid.id: - if i > 16: - hex_string += hex(i)[-2:] - else: - hex_string += "0" + hex(i)[-1:] - if hex_string in tcr_rx_reqids: - index = tcr_rx_reqids.index(hex_string) - try: - min_time = min(reqid_v2x_timestamps[index][2:]) # Min timestamp of V2XHub transmitting a TCM for a msgnum of that reqid - max_time = max(reqid_v2x_timestamps[index][2:]) # Max timestamp of V2XHub transmitting a TCM for a msgnum of that reqid - #print("TCR " + str(num) + " broadcasted at time " + str(t.to_sec())) - time_cmv_to_v2x = reqid_v2x_timestamps[index][1] - float(t.to_sec()) - print("TCR " + str(num) + ": " + str(hex_string) + " received by V2XHub " + str(time_cmv_to_v2x) + " sec after cmv broadcast; TCM transmitted " + str(min_time) + " to " + str(max_time) + " afterwards") - #print("Vehicle broadcasted TCR at " + str(t.to_sec()) + ", V2XHub RSU received TCR at " + str(reqid_v2x_timestamps[index][1])) - except: - print("TCR " + str(num) + ": " + str(reqid_v2x_timestamps[index])) - #print("From tcr_rx_reqids: " + str(tcr_rx_reqids[index])) - #else: - # print("TCR " + str(num) + ": " + str(hex_string) + " not received by V2XHub") - - tcr_reqids_times.append(t) - num += 1 - - #print("Now TCMs received: ") - num = 0 - tcm_reqids = [] - has_received_unknown_TCM = False - - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - reqid = [int(i) for i in msg.tcm_v01.reqid.id] - if reqid not in tcm_reqids: - tcm_reqids.append(reqid) - if reqid in tcr_reqids: - index = tcr_reqids.index(reqid) - duration_tcr_to_tcm = (t - tcr_reqids_times[index]).to_sec() - hex_string = "" - for i in msg.tcm_v01.reqid.id: - if i > 16: - hex_string += hex(i)[-2:] - else: - hex_string += "0" + hex(i)[-1:] - - print("TCM " + str(num) + " was TCR # " + str(index) + ". Time from TCR to TCM was " + str(duration_tcr_to_tcm) + " seconds. Reqid: " + str(hex_string)) - else: - if not has_received_unknown_TCM: - #print("RECEIVED TCM THAT WAS NOT A BROADCASTED TCR") - has_received_unknown_TCM = True - - num += 1 - -# Helper function: Converts time from HH::MM::SS format of a given day to its Unix Epoch Time -# Note: This is used because rosbags use Unix Epoch Time and some V2XHub and CARMA Cloud logs use HH::MM::SS EST format -def get_system_time(time): - hours = float(time[0:2]) - 4.0 # The '4.0' is hardcoded since CarmaCloud time is 4 hours ahead of the EST time (at least for the initial data set analyzed) - minutes = float(time[3:5]) - seconds = float(time[6:8]) - ms = float(time[9:12]) - - #system_time = 1650513600.0 # HARDCODED (use for tests conducted on April 21st, 2022; this represents the epoch time at start of that day) - system_time = 1650600000.0 # HARDCODED (use for tests conducted on April 22nd; this represents the epoch time at start of that day) - system_time += 60*60*hours + 60*minutes + seconds + 0.001*ms - - return system_time - -# Helper function for processing TCR receive and TCM send times from a CARMA Cloud tomcat log text file -# NOTE: Function developed for April 22nd version of CARMA Cloud tomcat logs; the text log ouputs may no longer -# match the assumptions made in this function -# Inputs: -# Source Folder: (String) Path to folder that contains tomcat log files -# tomcat_log_files: (String) List of file names to be processed -# empty_val: (integer) Integer value that indicates an element in the returned lists are empty -# Outputs: -# all_cc_data: List with each row being [Received TCR reqid, TCR receive time, duration until TCM msgnum 1 sent, duration until TCM msgnum 2 sent, etc....] -# Note: List is in order of received TCR reqids -# cc_tcr_rx_reqids: List of each received TCR reqid in order that CARMA Cloud received it -def process_tomcat_logs(source_folder, tomcat_log_files, empty_val): - # Each row of all_cc_data will be [TCR reqid, TCR receive time, duration until TCM msgnum 1 sent, duration until TCM msgnum 2 sent, ....etc....] - # Note: Populated in order of received TCR reqids; assumes 9 TCMs are sent for each TCR (this is the number used for Freight WZ testing) - all_cc_data = [] - - # List of each TCR received by CARMA Cloud in order (lowercase) - cc_tcr_rx_reqids = [] - - # Process each file contained within the provided tomcat_log_files list - for filename in tomcat_log_files: - print("Processing tomcat log file: " + str(filename)) - - # Parse each line in the tomcat log file and update all_cc_data and cc_tcr_rx_reqids based on TCR- and TCM-related logs - with open(str(source_folder) + "/" + filename, newline='') as f: - while(True): - line = f.readline() - - if not line: - break - else: - if "TCR" in line: - # Process a line that describes a received TCR by CARMA Cloud (from V2XHub) - - tcr_reqid = line[97:113] - time = line[7:19] - system_time = get_system_time(time) # Convert HH::MM::SS to Unix Epoch Time - - # NOTE: This assumes there are 9 TCMs being sent out for this test - all_cc_data.append([str(tcr_reqid).lower(), system_time, empty_val, empty_val, empty_val, empty_val, empty_val, empty_val, empty_val, empty_val, empty_val]) - if tcr_reqid in cc_tcr_rx_reqids: - print("WARNING: Duplicate TCR reqid found in tomcat log file " + str(filename) +": " + str(tcr_reqid)) - - cc_tcr_rx_reqids.append(tcr_reqid.lower()) - #print(str(tcr_reqid.lower()) + " added to cc_tcr_rx_reqids") - - if "TCM 404" in line: - # Process a line that describes a TCM sent out by CARMA Cloud (to V2XHub) - - tcm_reqid = (line[109:125]).lower() - time = line[7:19] - system_time = get_system_time(time) # Convert HH::MM::SS to Unix Epoch Time - msgnum = int(line[177:178]) - - index = cc_tcr_rx_reqids.index(tcm_reqid) - #print("New TCM: " + str(tcm_reqid) + " msgnum " + str(msgnum) + " at time " + str(system_time) + " in response to received TCR #" + str(index)) - - if all_cc_data[index][msgnum+1] == empty_val: - all_cc_data[index][msgnum+1] = system_time - all_cc_data[index][1] - else: - print("Duplicate TCM: " + str(tcm_reqid) + " msgnum " + str(msgnum) + " at time " + str(system_time) + " for TCR #" + str(index)) - - min_times = [] - count_below_one_sec = 0 - min_time = 9999 - max_time = 0.0 - for row in all_cc_data: - if min(row[2:]) != empty_val: - min_times.append(min(row[1:])) - if min(row[2:]) < min_time: - min_time = min(row[1:]) - if max(row[2:]) != empty_val: - if max(row[2:]) > max_time: - max_time = max(row[2:]) - if min(row[2:]) <= 1.0: - count_below_one_sec += 1 - avg_time_cc = sum(min_times) / len(min_times) - pct_below_one_sec = (float(count_below_one_sec / len(min_times))) * 100.0 - - variance = sum([((x - avg_time_cc) ** 2) for x in min_times]) / len(min_times) - std_dev = variance ** 0.5 - - print("Average duration between CARMA Cloud receiving TCR from CS and sending first TCM: " + str(avg_time_cc)) - print("Max duration: " + str(max_time)) - print("Min duration: " + str(min_time)) - print("Standard deviation: " + str(std_dev)) - print("Percentage below 1 sec: " + str(pct_below_one_sec) + "%") - - return all_cc_data, cc_tcr_rx_reqids - -def process_v2x_receive_and_broadcast_logs(source_folder, tcr_receive_csv_filenames, tcm_broadcast_csv_filenames, mobop_receive_csv_filenames, cc_tcr_rx_reqids, all_cc_data, empty_val): - # Read in V2XHub TCR Receiving Data from CSV Files - tcr_data = [] - for filename in tcr_receive_csv_filenames: - with open(str(source_folder) + "/rsu_tcr_receive_data/" + filename, newline='') as f: - reader = csv.reader(f) - tcr_data += list(reader)[1:] - - # Read in V2XHub TCM Transmission Data from CSV Files - tcm_data = [] - for filename in tcm_broadcast_csv_filenames: - with open(str(source_folder) + "/" + filename, newline='') as f: - reader = csv.reader(f) - tcm_data += list(reader)[1:] - - # Create merged list of relevant V2XHub TCR Receiving & TCM Broadcasting data: - # reqid_v2x_timestamps is a list of lists: For each list, index 0 is reqid; 1 is tcr receive time; 2-10 are duration until FIRST tcm tx time for msgnums 0 to 9 - reqid_v2x_timestamps = [] - tcr_rx_times = [float(i[0]) for i in tcr_data[1:]] # Create list of times (in order) of each received TCR - tcr_rx_reqids = [str(i[2]) for i in tcr_data[1:]] # Create list of reqids (in order) of each received TCR - for i in range(0, len(tcr_rx_times)): - # Initial creation of each list in reqid_v2x_timestamps with first two indexes set to true values; all other indices set to temp '99999' - reqid_v2x_timestamps.append([tcr_rx_reqids[i], tcr_rx_times[i], empty_val, empty_val, empty_val, empty_val, empty_val, empty_val, empty_val, empty_val, empty_val]) - - for i in range(0, len(tcm_data)): - reqid = tcm_data[i][2] - if reqid in tcr_rx_reqids: - index = tcr_rx_reqids.index(reqid) - msgnum = tcm_data[i][3] - if reqid_v2x_timestamps[index][1 + int(msgnum)] == empty_val: - time_tcm_broadcasted = tcm_data[i][0] - time_tcr_received = reqid_v2x_timestamps[index][1] - - # Add duration between received TCR and this TCM msgnum broadcast to reqid_v2x_timestamps - reqid_v2x_timestamps[index][1 + int(msgnum)] = float(time_tcm_broadcasted) - time_tcr_received - - # Initial processing of reqid_v2x_timestamps to gather time duration data for communication flow between V2XHub TCR Receive <-> V2XHub TCM Broadcast - for row in reqid_v2x_timestamps: - if row[0] in cc_tcr_rx_reqids: - print("---") - v2x_tcr_receive_time = row[1] - cc_index = cc_tcr_rx_reqids.index(row[0]) - cc_row = all_cc_data[cc_index] - cc_tcr_receive_time = cc_row[1] - duration_v2x_to_cc_tcr = cc_tcr_receive_time - v2x_tcr_receive_time - - formatted_cc_row = [ float(elem) for elem in cc_row[1:] ] - formatted_cc_row = [ '%.2f' % elem for elem in cc_row[1:] ] - - print("TCR Reqid " + str(row[0]) + " took " + str(duration_v2x_to_cc_tcr) + " between V2XHub RSU receiving it and CC receiving it.") - - formatted_total_rtt_row = [float(elem) for elem in row[1:]] - formatted_total_rtt_row = [ '%.2f' % elem for elem in row[1:] ] - print("TCR -> TCM total times V2X " + str(formatted_total_rtt_row)) - - print("CC Receive to CC Send TCM: " + str(formatted_cc_row)) - - # For each msgnum, subtract the duration_v2x_to_cc_tcr and the corresponding time that it was within CC - for i in range(2, len(row)): - row[i] = row[i] - (duration_v2x_to_cc_tcr + cc_row[i]) - - formatted_v2x_row = [float(elem) for elem in row[1:]] - formatted_v2x_row = [ '%.2f' % elem for elem in row[1:] ] - - print("V2X Receive to TCM broad: " + str(formatted_v2x_row)) - else: - print("TCR Reqid " + str(row[0]) + " not received by Carma Cloud") - continue - - # Process received MobilityOperation TCM acknowledgement messages - mo_data = [] - for filename in mobop_receive_csv_filenames: - with open(str(source_folder) + "/rsu_mobop_receive_data/" + filename, newline='') as f: - reader = csv.reader(f) - mo_data += list(reader)[1:] - - #[reqid, num_broadcast_1, num_broadcast_2, num_broadcast_3, ..] - #[reqid, time_receive_1, time_receive_2, time_receive_3, ...] - ack_v2x_timestamps = [] - tcm_v2x_tx_before_ack_counts = [] - tcm_v2x_tx_after_ack_counts = [] - tcm_v2x_tx_times = [] - tcm_v2x_tx_frequencies = [] - for i in range(0, len(reqid_v2x_timestamps)): - ack_v2x_timestamps.append([reqid_v2x_timestamps[i][0], 0, 0, 0, 0, 0, 0, 0, 0, 0]) - tcm_v2x_tx_before_ack_counts.append([reqid_v2x_timestamps[i][0], 0, 0, 0, 0, 0, 0, 0, 0, 0]) - tcm_v2x_tx_after_ack_counts.append([reqid_v2x_timestamps[i][0], 0, 0, 0, 0, 0, 0, 0, 0, 0]) - tcm_v2x_tx_times.append([reqid_v2x_timestamps[i][0], [], [], [], [], [], [], [], [], []]) - tcm_v2x_tx_frequencies.append([reqid_v2x_timestamps[i][0], 0, 0, 0, 0, 0, 0, 0, 0, 0]) - - for i in range(1, len(mo_data)): - try: - reqid = mo_data[i][3] - msgnum = mo_data[i][4] - time_ack = float(mo_data[i][0]) - index = tcr_rx_reqids.index(reqid) - if ack_v2x_timestamps[index][int(msgnum)] == 0: - ack_v2x_timestamps[index][int(msgnum)] = time_ack - except: - continue - - # Now create a [reqid, num_broadcast_tcm1_before_ack, num_broadcast_tcm2_before_ack, ...] - for i in range(0, len(tcm_data)): - # Check if TCM reqid matches a received TCR req id - if tcm_data[i][2] in tcr_rx_reqids: - index = tcr_rx_reqids.index(tcm_data[i][2]) - msgnum = tcm_data[i][3] - time = float(tcm_data[i][0]) # Time that V2XHub RSU broadcasted TCM - - # If TCM was broadcasted before the time of the received TCM Ack, increase TCM tx number for that msgnum - tcm_v2x_tx_times[index][int(msgnum)].append(time) - - # No ACK was ever received - if ack_v2x_timestamps[index][int(msgnum)] == 0: - tcm_v2x_tx_before_ack_counts[index][int(msgnum)] += 1 - - # TCM was broadcasted before ACK was received - elif time < ack_v2x_timestamps[index][int(msgnum)]: - tcm_v2x_tx_before_ack_counts[index][int(msgnum)] += 1 - - # TCM was broadcasted after ACK was received - else: - tcm_v2x_tx_after_ack_counts[index][int(msgnum)] += 1 - - # Calculate the TCM tx frequencies for each TCM reqid's msgnum: - total_freq = 0.0 - min_freq = 9999999 - max_freq = 0.0 - freq_count = 0 - for i in range(0, len(tcm_v2x_tx_frequencies)): - for msgnum in range(1, 10): - tx_count = tcm_v2x_tx_before_ack_counts[i][msgnum] + tcm_v2x_tx_after_ack_counts[i][msgnum] - tx_time_diffs = 0.0 - for k in range(1, len(tcm_v2x_tx_times[i][msgnum])): - tx_time_diffs += tcm_v2x_tx_times[i][msgnum][k] - tcm_v2x_tx_times[i][msgnum][k-1] - if tx_count >= 2: - tcm_v2x_tx_frequencies[i][msgnum] = 1.0 / (float(tx_time_diffs) / float(tx_count-1)) - total_freq += tcm_v2x_tx_frequencies[i][msgnum] - freq_count += 1 - - if tcm_v2x_tx_frequencies[i][msgnum] < min_freq: - min_freq = tcm_v2x_tx_frequencies[i][msgnum] - if tcm_v2x_tx_frequencies[i][msgnum] > max_freq: - max_freq = tcm_v2x_tx_frequencies[i][msgnum] - - print("Quantity TCM msgnum pubs after ACK first received for that msgnum: ") - num = 0 - for i in tcm_v2x_tx_after_ack_counts: - if max(i[1:]) >= 0: - print("Before ACKs received: " + str(tcm_v2x_tx_before_ack_counts[num])) - print("After ACKs received: " + str(i)) - print("TCM tx frequencies: " + str(tcm_v2x_tx_frequencies[num])) - num += 1 - - avg_freq_tcm_tx = total_freq / float(freq_count) - print("Average frequency of TCM broadcasts: " + str(avg_freq_tcm_tx)) - print("Min frequency: " + str(min_freq)) - print("Max frequency: " + str(max_freq)) - - return tcr_rx_times, tcr_rx_reqids, reqid_v2x_timestamps - -# Main Function; run all tests from here -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - source_folder = sys.argv[1] - - # Re-direct the output of print() to a specified .txt file: - orig_stdout = sys.stdout - current_time = datetime.datetime.now() - #text_log_filename = "Results_" + str(current_time) + ".txt" - #text_log_file_writer = open(text_log_filename, 'w') - #sys.stdout = text_log_file_writer - - # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - #csv_results_filename = "Results_" + str(current_time) + ".csv" - #csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - #csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", - # "FWZ-1", "FWZ-2", "FWZ-3", "FWZ-4", "FWZ-5", - # "FWZ-6", "FWZ-7", "FWZ-8", "FWZ-9", "FWZ-10", - # "FWZ-11", "FWZ-12", "FWZ-13", "FWZ-14", "FWZ-15", - # "FWZ-16", "FWZ-17", "FWZ-18", "FWZ-19", "FWZ-20", - # "FWZ-21", "FWZ-22", "FWZ-23", "FWZ-24", "FWZ-25", - # "FWZ-26", "FWZ-27", "FWZ-28", "FWZ-29", "FWZ-30", - # "FWZ-31", "FWZ-32"]) - - white_truck_bag_files = ["Silver_Truck_April20-April27/_2022-04-22-13-50-06.bag", - "Silver_Truck_April20-April27/_2022-04-22-13-41-54.bag", - "Silver_Truck_April20-April27/_2022-04-22-14-11-26.bag", - "Silver_Truck_April20-April27/_2022-04-25-15-38-11.bag", - "Silver_Truck_April20-April27/_2022-04-25-16-50-46.bag", - "Silver_Truck_April20-April27/_2022-04-25-17-10-20.bag", - "Silver_Truck_April20-April27/_2022-04-25-17-44-17.bag", - "Silver_Truck_April20-April27/_2022-04-25-18-54-03.bag", - "Silver_Truck_April20-April27/_2022-04-25-19-06-33.bag", - "Silver_Truck_April20-April27/_2022-04-25-21-16-09.bag", - "Silver_Truck_April20-April27/_2022-04-25-21-29-52.bag"] - - - tomcat_log_files = ["carmacloud-20220422.txt"] - - # In this script, any list element with a value that matches 'empty_val' is ignored - empty_val = 99999 - - # Process the CARMA Cloud tomcat log files to produce the two following data structures - # all_cc_data: List with each row being [Received TCR reqid, TCR receive time, duration until TCM msgnum 1 sent, duration until TCM msgnum 2 sent, etc....] - # Note: List is in order of received TCR reqids - # cc_tcr_rx_reqids: List of each received TCR reqid in order that CARMA Cloud received it - all_cc_data, cc_tcr_rx_reqids = process_tomcat_logs(source_folder, tomcat_log_files, empty_val) - - tcr_receive_csv_filenames = ["merged-tcr-rx-0421-to-0425.csv"] - tcm_broadcast_csv_filenames = ["merged-tcm-tx-0421-to-0425.csv"] - mobop_receive_csv_filenames = ["merged-mobop-rx-0421-to-0425.csv"] - tcr_rx_times, tcr_rx_reqids, reqid_v2x_timestamps = process_v2x_receive_and_broadcast_logs(source_folder, tcr_receive_csv_filenames, tcm_broadcast_csv_filenames, mobop_receive_csv_filenames, cc_tcr_rx_reqids, all_cc_data, empty_val) - - # Concatenate all Freight Work Zone bag file lists into one list - WZ_bag_files = white_truck_bag_files - - # Loop to conduct data anlaysis on each bag file: - for bag_file in WZ_bag_files: - print("*****************************************************************") - print("Processing new bag: " + str(bag_file)) - if bag_file in white_truck_bag_files: - print("White Truck Freight Work Zone Test Case") - else: - print("Unknown bag file being processed.") - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - #sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(WZ_bag_files.index(bag_file) + 1) + " of " + str(len(WZ_bag_files)) + ")") - #sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - bag = rosbag.Bag(bag_file_path) - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - # Get the rosbag times associated with entering and exiting the active geofence - print("Getting geofence times at " + str(datetime.datetime.now())) - time_enter_geofence, time_exit_geofence, found_geofence_times = get_geofence_entrance_and_exit_times(bag) - print("Got geofence times at " + str(datetime.datetime.now())) - #if (not found_geofence_times): - # print("Could not find geofence entrance and exit times in bag file.") - # continue - - # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test - print("Getting engagement times at " + str(datetime.datetime.now())) - time_test_start_engagement, time_test_end_engagement, found_test_times = get_test_case_engagement_times(bag, time_enter_geofence, time_exit_geofence) - print("Started engagement at " + str(time_test_start_engagement.to_sec())) - print("Ended engagement at " + str(time_test_end_engagement.to_sec())) - if (not found_test_times): - print("Could not find test case engagement start and end times in bag file.") - continue - - print("Spent " + str((time_test_end_engagement - time_test_start_engagement).to_sec()) + " seconds engaged.") - - original_speed_limit_ms = get_route_original_speed_limit(bag, time_test_start_engagement) # Units: m/s - print("Original Speed Limit is " + str(original_speed_limit_ms) + " m/s") - - # OPTIONAL: Generate Plots: - - # Generate vehicle speed plot for the rosbag - #generate_speed_plot(bag, time_test_start_engagement, time_test_end_engagement, bag_file) - - # Generate cross-track error plot for the rosbag - #generate_crosstrack_plot(bag, time_test_start_engagement, time_test_end_engagement, bag_file) - - # Initialize success flags - fwz_1_result = False - fwz_2_result = False - fwz_3_result = False - fwz_4_result = False - fwz_5_result = False - fwz_6_result = False - fwz_7_result = False - fwz_8_result = False - fwz_9_result = False - fwz_10_result = False - fwz_11_result = False - fwz_12_result = False - fwz_13_result = False - fwz_14_result = False - fwz_15_result = False - fwz_16_result = False - fwz_17_result = False - fwz_18_result = False - fwz_19_result = False - fwz_20_result = False - fwz_21_result = False - fwz_22_result = False - fwz_23_result = False - fwz_24_result = False - fwz_25_result = False - fwz_26_result = False - fwz_27_result = False - fwz_28_result = False - fwz_29_result = False - fwz_30_result = False - fwz_31_result = False - fwz_32_result = False - - # Metric FWZ-1 (geofenced area is a part of the initial route) - # Metric FWZ-8 (final route does not include the closed lanelets)) - closed_lanelets = [10801, 10802] - fwz_1_result, fwz_8_result = check_geofence_in_initial_route(bag, closed_lanelets) - - - fwz_2_result, time_start_steady_state_before_geofence = check_start_steady_state_before_geofence(bag, time_test_start_engagement, time_enter_geofence, original_speed_limit_ms) - - advisory_speed_limit_ms = 11.176 # 11.176 m/s is 25 mph - fwz_7_result = check_in_geofence_speed_limits(bag, time_enter_geofence, time_exit_geofence, advisory_speed_limit_ms) - - restricted_lanelets = [20801, 20802] - fwz_9_result = check_restricted_lane_tcm_received(bag, time_test_start_engagement, time_test_end_engagement, restricted_lanelets) - - expected_number_tcms = 9 - fwz_10_result = check_tcm_acknowledgements(bag, time_test_start_engagement, time_test_end_engagement, expected_number_tcms) - - fwz_12_result = check_reroute_duration(bag, time_test_start_engagement, time_test_end_engagement) - - num_expected_lanechanges = 4 - fwz_15_result = check_lanechange_duration(bag, time_test_start_engagement, time_test_end_engagement, num_expected_lanechanges) - - fwz_23_result, time_begin_deceleration_in_geofence = check_time_to_begin_deceleration(bag, time_test_start_engagement, time_test_end_engagement, advisory_speed_limit_ms) - - workzone_lanelet_id = 30801 - fwz_24_result = check_speed_before_workzone(bag, time_test_start_engagement, time_test_end_engagement, workzone_lanelet_id, advisory_speed_limit_ms) - - fwz_25_result = check_deceleration_for_geofence(bag, time_test_start_engagement, time_test_end_engagement, time_begin_deceleration_in_geofence, advisory_speed_limit_ms) - - fwz_26_result, time_begin_acceleration_after_geofence = check_time_to_begin_acceleration(bag, time_begin_deceleration_in_geofence, time_test_end_engagement, original_speed_limit_ms) - - fwz_27_result = check_acceleration_after_geofence(bag, time_begin_acceleration_after_geofence, time_test_end_engagement, original_speed_limit_ms) - - fwz_29_result = check_steady_state_after_geofence(bag, time_begin_acceleration_after_geofence, time_test_end_engagement, original_speed_limit_ms) - - check_tcm_response_time(bag, time_enter_geofence, tcr_rx_times, tcr_rx_reqids, reqid_v2x_timestamps) - - - # Get vehicle type that this bag file is from - vehicle_name = "Unknown" - if bag_file in white_truck_bag_files: - vehicle_name = "White Truck" - else: - vehicle_name = "N/A" - - # Get test type that this bag file is for - vehicle_role = "Freight Work Zone" - - # Write simple pass/fail results to .csv file for appropriate row: - #csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, - # fwz_1_result, fwz_2_result, fwz_3_result, fwz_4_result, fwz_5_result, - # fwz_6_result, fwz_7_result, fwz_8_result, fwz_9_result, fwz_10_result, - # fwz_11_result, fwz_12_result, fwz_13_result, fwz_14_result, fwz_15_result, - # fwz_16_result, fwz_17_result, fwz_18_result, fwz_19_result, fwz_20_result, - # fwz_21_result, fwz_22_result, fwz_23_result, fwz_24_result, fwz_25_result, - # fwz_26_result, fwz_27_result, fwz_28_result, fwz_29_result, fwz_30_result, - # fwz_31_result, fwz_32_result]) - - - #sys.stdout = orig_stdout - #text_log_file_writer.close() - return - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_ihp2_rosbags.py b/engineering_tools/data_processing/analyze_ihp2_rosbags.py deleted file mode 100644 index af3cd318e2..0000000000 --- a/engineering_tools/data_processing/analyze_ihp2_rosbags.py +++ /dev/null @@ -1,1276 +0,0 @@ -#!/usr/bin/python3 - -# 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. - -from inspect import TPFLAGS_IS_ABSTRACT -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime -import math - -# HOW TO USE SCRIPT: -# Run the following in a terminal to download dependencies: -# sudo add-apt-repository ppa:deadsnakes/ppa -# sudo apt-get update -# sudo apt install python3.7 -# python3.7 -m pip install --upgrade pip -# python3.7 -m pip install matplotlib -# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg -# python3.7 -m pip install lz4 -# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ -# In terminal, navigate to the directory that contains this python script and run the following: -# python3.7 analyze_ihp2_rosbags.py - -def generate_two_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, time_starting_vehicle_start_engagement, time_starting_vehicle_end_engagement, veh_1_speed_limit_zone_change_times, test_type, test_num): - if test_type == "SH-MID" or test_type == "SH-HIGH": - # First vehicle to engage is vehicle 2 - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - vehicle_1_speed_times = [] - vehicle_1_speeds = [] - for topic, msg, t in vehicle_1_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - vehicle_1_speed_times.append((t-time_start).to_sec()) - vehicle_1_speeds.append(msg.twist.linear.x) # Current speed in m/s - - vehicle_2_speed_times = [] - vehicle_2_speeds = [] - for topic, msg, t in vehicle_2_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - vehicle_2_speed_times.append((t-time_start).to_sec()) - vehicle_2_speeds.append(msg.twist.linear.x) # Current speed in m/s - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot vehicle 1 speed (m/s) vs. time - ax.plot(vehicle_1_speed_times, vehicle_1_speeds, 'g:', label='Vehicle 1 Speed (m/s)') - - # Plot vehicle 2 speed (m/s) vs. time - ax.plot(vehicle_2_speed_times, vehicle_2_speeds, 'b:', label='Vehicle 2 Speed (m/s)') - - # Optional: Plot a vertical bar at the time of each speed limit change time - if test_type == "SH-MID": - for i in range(0, len(veh_1_speed_limit_zone_change_times)): - if i == 0: - ax.axvline(x = (veh_1_speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r', label = 'Leader Speed Zone Entrance Times (In Order: 15.6, 11.3, 10.5, 9.7, 9.4, 15.6 in m/s)') - else: - ax.axvline(x = (veh_1_speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r') - elif test_type == "SH-HIGH": - for i in range(0, len(veh_1_speed_limit_zone_change_times)): - if i == 0: - ax.axvline(x = (veh_1_speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r', label = 'Leader Speed Zones Entrance Times (In Order: 15.6, 9.1, 8.3, 7.5, 7.2, 15.6 in m/s)') - else: - ax.axvline(x = (veh_1_speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(test_type) + " Test Case Run " + str(test_num) + " Vehicle Speeds") # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Engagement for Starting Vehicle") # Plot X Title - ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title - - # Option 1: Save the plot - filename = str(test_type) + "_Run_" + str(test_num) + "_Vehicle_Speeds.png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -def generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_starting_vehicle_start_engagement, time_starting_vehicle_end_engagement, test_type, test_num): - if test_type == "SLR" or test_type == "ALR": - # First vehicle to engage is vehicle 1 - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - vehicle_1_speed_times = [] - vehicle_1_speeds = [] - for topic, msg, t in vehicle_1_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - vehicle_1_speed_times.append((t-time_start).to_sec()) - vehicle_1_speeds.append(msg.twist.linear.x) # Current speed in m/s - - - vehicle_2_speed_times = [] - vehicle_2_speeds = [] - for topic, msg, t in vehicle_2_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - vehicle_2_speed_times.append((t-time_start).to_sec()) - vehicle_2_speeds.append(msg.twist.linear.x) # Current speed in m/s - - vehicle_3_speed_times = [] - vehicle_3_speeds = [] - for topic, msg, t in vehicle_3_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - vehicle_3_speed_times.append((t-time_start).to_sec()) - vehicle_3_speeds.append(msg.twist.linear.x) # Current speed in m/s - - elif test_type == "SLF" or test_type == "ALF": - # First vehicle to engage is vehicle 2 - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - vehicle_2_speed_times = [] - vehicle_2_speeds = [] - for topic, msg, t in vehicle_2_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - vehicle_2_speed_times.append((t-time_start).to_sec()) - vehicle_2_speeds.append(msg.twist.linear.x) # Current speed in m/s - - vehicle_1_speed_times = [] - vehicle_1_speeds = [] - for topic, msg, t in vehicle_1_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - vehicle_1_speed_times.append((t-time_start).to_sec()) - vehicle_1_speeds.append(msg.twist.linear.x) # Current speed in m/s - - vehicle_3_speed_times = [] - vehicle_3_speeds = [] - for topic, msg, t in vehicle_3_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = time_starting_vehicle_end_engagement): # time_start_engagement+time_duration): - vehicle_3_speed_times.append((t-time_start).to_sec()) - vehicle_3_speeds.append(msg.twist.linear.x) # Current speed in m/s - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot vehicle 1 speed (m/s) vs. time - ax.plot(vehicle_1_speed_times, vehicle_1_speeds, 'g:', label='Vehicle 1 Speed (m/s)') - - # Plot vehicle 2 speed (m/s) vs. time - ax.plot(vehicle_2_speed_times, vehicle_2_speeds, 'b:', label='Vehicle 2 Speed (m/s)') - - # Plot vehicle 2 speed (m/s) vs. time - ax.plot(vehicle_3_speed_times, vehicle_3_speeds, 'r:', label='Vehicle 3 Speed (m/s)') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(test_type) + " Test Case Run " + str(test_num) + " Vehicle Speeds") # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Engagement for Starting Vehicle") # Plot X Title - ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title - - # Option 1: Save the plot - filename = str(test_type) + "_Run_" + str(test_num) + "_Vehicle_Speeds.png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -def generate_speed_plot(bag, time_start_engagement, time_end_engagement, bag_file_name, speed_limit_zone_change_times, test_type, vehicle_number): - # Speed command: /hardware_interface/arbitrated_speed_commands: msg.speed (m/s) - # True Speed: /hardware_interface/vehicle/twist: msg.twist.linear.x (m/s) - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - true_vehicle_speed_times = [] - true_vehicle_speeds = [] - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - true_vehicle_speed_times.append((t-time_start).to_sec()) - true_vehicle_speeds.append(msg.twist.linear.x) # Current speed in m/s - - # TODO: UPDATE THE TOPIC USED FOR THE LEXUS/PACMOD SPEED COMMANDS - # Get the commanded vehicle speed (m/s) and the associated time with each data point - first = True - cmd_vehicle_speed_times = [] - cmd_vehicle_speeds = [] - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/arbitrated_speed_commands'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - cmd_vehicle_speed_times.append((t-time_start).to_sec()) - cmd_vehicle_speeds.append(msg.speed) # Commanded speed in m/s - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot commanded vehicle speed (m/s) vs. time - ax.plot(cmd_vehicle_speed_times, cmd_vehicle_speeds, 'g:', label='Commanded Speed (m/s)') - - # Plot true vehicle speed (m/s) vs. time - ax.plot(true_vehicle_speed_times, true_vehicle_speeds, 'b--', label='Actual Speed (m/s)') - - # Optional: Plot a vertical bar at the time of each speed limit change time - if test_type == "SH-MID": - for i in range(0, len(speed_limit_zone_change_times)): - if i == 0: - ax.axvline(x = (speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r', label = 'Speed Zone Start Times (In Order: 15.6, 11.3, 10.5, 9.7, 9.4, 15.6 in m/s)') - else: - ax.axvline(x = (speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r') - elif test_type == "SH-HIGH": - for i in range(0, len(speed_limit_zone_change_times)): - if i == 0: - ax.axvline(x = (speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r', label = 'Speed Zones Start Times (In Order: 15.6, 9.1, 8.3, 7.5, 7.2, 15.6 in m/s)') - else: - ax.axvline(x = (speed_limit_zone_change_times[i] - time_start).to_sec(), color = 'r') - - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Speed (Commanded and Actual) -- Vehicle " + str(vehicle_number)) # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Engagement") # Plot X Title - ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title - - # Option 1: Save the plot - filename = "Speed_" + bag_file_name + ".png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -def generate_platooning_plot(bag, time_start_platooning, bag_file_name, test_type, vehicle_number): - - actual_gaps = [] - desired_gaps = [] - times = [] - first = True - for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_start_platooning): - if first: - time_start = t - first = False - continue - - actual_gaps.append(msg.actual_gap) - desired_gaps.append(msg.desired_gap) - times.append((t-time_start).to_sec()) - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot actual gaps (meters) vs. time - ax.plot(times, actual_gaps, 'g:', label='Actual Gap (meters)') - - # Plot desired gaps (meters) vs. time - ax.plot(times, desired_gaps, 'r--', label='Desired Gap (meters)') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Platooning Information -- Vehicle " + str(vehicle_number)) # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Following in Platoon") # Plot X Title - ax.set_ylabel("Meters") # Plot Y Title - - # Option 1: Save the plot - filename = "Platooning_Information_" + bag_file_name + ".png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -def generate_crosstrack_plot(bag, time_start_engagement, time_end_engagement, bag_file_name): - # Crosstrack Error: /message/route_state (meters)) - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - crosstrack_times = [] - crosstrack_values = [] - # Note: This topic name assumes a pacmod controller is being used (freightliners or lexus) - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_end_engagement): - if first: - time_start = t - first = False - continue - - crosstrack_times.append((t-time_start).to_sec()) - crosstrack_values.append(msg.cross_track) # Crosstrack error in meters - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot crosstrack error (meters) vs. time - ax.plot(crosstrack_times, crosstrack_values, 'g:', label='Crosstrack Error (meters)') - - # Optional: Plot a horizontal bar at a positive value for reference - ax.axhline(y = 0.95, color = 'r', label = '+/- 0.95 Meters (For Reference)') - - # Optional: Plot a horizontal bar at a negative value for reference - ax.axhline(y = -0.95, color = 'r') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Crosstrack Error") # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Engagement") # Plot X Title - ax.set_ylabel("Crosstrack Error (meters)") # Plot Y Title - - # Option 1: Save the plot - filename = "Crosstrack_" + bag_file_name + ".png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section -def get_test_case_engagement_times(bag): - # Initialize system engagement start and end times - time_start_engagement = rospy.Time() - time_stop_engagement = rospy.Time() - - # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section - is_engaged = False - found_starting_engagement_time = False - found_ending_engagement_time = False - for topic, msg, t in bag.read_messages(topics=['/guidance/state']): - # If entering engagement, track this start time - if (msg.state == 4 and not is_engaged): - found_starting_engagement_time = True - time_start_engagement = t - is_engaged = True - - # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance - # state can be published. - if (msg.state == 4): - time_last_engaged = t - - # Log time that engagements ends - elif (msg.state != 4 and is_engaged): - found_ending_engagement_time = True - is_engaged = False - time_stop_engagement = t - - # If CARMA ended engagement before guidance state could be updated, update time_stop_engagement - if found_starting_engagement_time and not found_ending_engagement_time: - time_stop_engagement = time_last_engaged - - found_engagement_times = False - if found_starting_engagement_time: - found_engagement_times = True - - return time_start_engagement, time_stop_engagement, found_engagement_times - -def analyze_route_speed_limits(bag): - print("New route analyzed: ") - current_lanelet = None - current_speed_limit = None - lanelet_speed_limits = [] - speed_limit_zone_change_times = [] - speed_limit_zone_values = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): - if msg.lanelet_id != current_lanelet: - lanelet_speed_limits.append(msg.speed_limit) - current_lanelet = msg.lanelet_id - - if current_speed_limit is None: - print("New speed limit " + str(msg.speed_limit) + " at time " + str(t.to_sec())) - speed_limit_zone_values.append(msg.speed_limit) - speed_limit_zone_change_times.append(t) - current_speed_limit = msg.speed_limit - else: - if abs(current_speed_limit - msg.speed_limit) > 0.1: - print("New speed limit " + str(msg.speed_limit) + " at time " + str(t.to_sec())) - speed_limit_zone_values.append(msg.speed_limit) - speed_limit_zone_change_times.append(t) - current_speed_limit = msg.speed_limit - - # Remove first element for zero speed limit - lanelet_speed_limits = lanelet_speed_limits[1:] - speed_limit_zone_change_times = speed_limit_zone_change_times[1:] - speed_limit_zone_values = speed_limit_zone_values[1:] - - print("Route speed limits in m/s (1 entry for each shortest-path lanelet in order): " + str(lanelet_speed_limits[1:])) - - return speed_limit_zone_values,speed_limit_zone_change_times - - -def leader_platoon_formation_analysis(bag, run_type, leader_id, follower_id): - - if run_type == "SLR": - has_received_initial_request = False - time_received_initial_request = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_request']): - if msg.m_header.recipient_id == leader_id and msg.m_header.sender_id == follower_id: - if msg.plan_type.type == 3: - has_received_initial_request = True - time_received_initial_request = t - print("Received initial request at " + str(t.to_sec())) - - has_sent_initial_ack = False - time_sent_initial_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_response']): - if msg.m_header.recipient_id == follower_id and msg.m_header.sender_id == leader_id: - if msg.plan_type.type == 3: - has_sent_initial_ack = True - time_sent_initial_ack = t - print("Sent initial ACK at " + str(t.to_sec())) - - - has_received_second_request = False - time_received_second_request = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_request']): - if msg.m_header.recipient_id == leader_id and msg.m_header.sender_id == follower_id: - if msg.plan_type.type == 4: - has_received_second_request = True - time_received_second_request = t - print("Received second request at " + str(t.to_sec())) - - - has_sent_second_ack = False - time_sent_second_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_response']): - if msg.m_header.recipient_id == follower_id and msg.m_header.sender_id == leader_id: - if msg.plan_type.type == 4: - has_sent_second_ack = True - time_sent_second_ack = t - print("Sent second ACK at " + str(t.to_sec())) - - - print("DEBUG: Time between initial request and ack: " + str((time_sent_initial_ack - time_received_initial_request).to_sec()) + " sec") - print("DEBUG: Time between initial request and ack: " + str((time_sent_second_ack - time_received_second_request).to_sec()) + " sec") - print("DEBUG: Time between initial request and second ack: " + str((time_sent_second_ack - time_received_initial_request).to_sec()) + " sec") - return - -def joiner_platoon_formation_analysis(bag, run_type, leader_id, follower_id, joiner_vehicle_number): - - has_sent_initial_request = False - time_sent_initial_request = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request']): - if msg.m_header.recipient_id == leader_id and msg.m_header.sender_id == follower_id: - if run_type == "SLR" or run_type == "SH-MID" or run_type == "SH-HIGH" or (run_type == "SLF" and joiner_vehicle_number == 3) \ - or (run_type == "ALR" and joiner_vehicle_number == 2) or (run_type == "ALF" and joiner_vehicle_number == 3): - if msg.plan_type.type == 3: - has_sent_initial_request = True - time_sent_initial_request = t - #print("DEBUG: Sent initial request at " + str(t.to_sec())) - - elif (run_type == "SLF" and joiner_vehicle_number == 1): - print("DEBUG: " + str(msg.m_header.sender_id) + " sending " + str(msg.m_header.recipient_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec())) - if msg.plan_type.type == 5: - has_sent_initial_request = True - time_sent_initial_request = t - #print("DEBUG: Sent initial request at " + str(t.to_sec())) - - elif (run_type == "ALR" and joiner_vehicle_number == 3) or (run_type == "ALF" and joiner_vehicle_number == 1): - print("DEBUG: " + str(msg.m_header.sender_id) + " sending " + str(msg.m_header.recipient_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec()) + " with params " + str(msg.strategy_params)) - if msg.plan_type.type == 8: - has_sent_initial_request = True - time_sent_initial_request = t - - has_received_initial_ack = False - time_received_initial_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response']): - if msg.m_header.recipient_id == follower_id and msg.m_header.sender_id == leader_id: - if run_type == "SLR" or run_type == "SH-MID" or run_type == "SH-HIGH" or (run_type == "SLF" and joiner_vehicle_number == 3) \ - or (run_type == "ALR" and joiner_vehicle_number == 2) or (run_type == "ALF" and joiner_vehicle_number == 3): - if msg.plan_type.type == 3 and msg.is_accepted: - has_received_initial_ack = True - time_received_initial_ack = t - #print("DEBUG: Received initial ACK at " + str(t.to_sec())) - - elif (run_type == "SLF" and joiner_vehicle_number == 1): - print("DEBUG: " + str(msg.m_header.recipient_id) + " receiving from " + str(msg.m_header.sender_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec()) + " with " + str(msg.is_accepted)) - if msg.plan_type.type == 5 and msg.is_accepted: - has_received_initial_ack = True - time_received_initial_ack = t - #print("DEBUG: Received initial ACK at " + str(t.to_sec())) - - elif (run_type == "ALR" and joiner_vehicle_number == 3) or (run_type == "ALF" and joiner_vehicle_number == 1): - print("DEBUG: " + str(msg.m_header.recipient_id) + " receiving from " + str(msg.m_header.sender_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec()) + " with " + str(msg.is_accepted)) - if msg.plan_type.type == 8 and msg.is_accepted: - has_received_initial_ack = True - time_received_initial_ack = t - - has_sent_second_request = False - time_sent_second_request = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request']): - if msg.m_header.recipient_id == leader_id and msg.m_header.sender_id == follower_id: - if run_type == "SLR" or run_type == "SH-MID" or run_type == "SH-HIGH" or (run_type == "SLF" and joiner_vehicle_number == 3) \ - or (run_type == "ALR" and joiner_vehicle_number == 2) or (run_type == "ALF" and joiner_vehicle_number == 3): - if msg.plan_type.type == 4: - has_sent_second_request = True - time_sent_second_request = t - #print("DEBUG: Sent second request at " + str(t.to_sec())) - - elif (run_type == "ALR" and joiner_vehicle_number == 3): - if msg.plan_type.type == 11: - has_sent_second_request = True - time_sent_second_request = t - - elif (run_type == "ALF" and joiner_vehicle_number == 1): - if msg.plan_type.type == 10: - has_sent_second_request = True - time_sent_second_request = t - - has_received_second_ack = False - time_received_second_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response']): - if msg.m_header.recipient_id == follower_id and msg.m_header.sender_id == leader_id: - if run_type == "SLR" or run_type == "SH-MID" or run_type == "SH-HIGH" or (run_type == "SLF" and joiner_vehicle_number == 3) \ - or (run_type == "ALR" and joiner_vehicle_number == 2) or (run_type == "ALF" and joiner_vehicle_number == 3): - if msg.plan_type.type == 4 and msg.is_accepted: - has_received_second_ack = True - time_received_second_ack = t - #print("DEBUG: Received second ACK at " + str(t.to_sec())) - - elif (run_type == "ALR" and joiner_vehicle_number == 3): - if msg.plan_type.type == 11 and msg.is_accepted: - has_received_second_ack = True - time_received_second_ack = t - - elif (run_type == "ALF" and joiner_vehicle_number == 1): - if msg.plan_type.type == 10 and msg.is_accepted: - has_received_second_ack = True - time_received_second_ack = t - - has_sent_third_request = False - time_sent_third_request = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request']): - if msg.m_header.recipient_id == leader_id and msg.m_header.sender_id == follower_id: - if (run_type == "ALR" and joiner_vehicle_number == 3): - if msg.plan_type.type == 4: - has_sent_third_request = True - time_sent_third_request = t - - has_received_third_ack = False - time_received_third_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response']): - if msg.m_header.recipient_id == follower_id and msg.m_header.sender_id == leader_id: - if (run_type == "ALR" and joiner_vehicle_number == 3): - if msg.plan_type.type == 4 and msg.is_accepted: - has_received_third_ack = True - time_received_third_ack = t - - has_received_second_request = False - time_received_second_request = rospy.Time() - has_received_third_request = False - time_received_third_request = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_request']): - print("DEBUG: " + str(msg.m_header.recipient_id) + " receiving from " + str(msg.m_header.sender_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec())) - if msg.m_header.recipient_id == follower_id and msg.m_header.sender_id == leader_id: - if (run_type == "SLF" and joiner_vehicle_number == 1): - if msg.plan_type.type == 6: - has_received_second_request = True - time_received_second_request = t - - elif (run_type == "ALF" and joiner_vehicle_number == 1): - print("DEBUG: " + str(msg.m_header.sender_id) + " sending " + str(msg.m_header.recipient_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec())) - if msg.plan_type.type == 6: - has_received_third_request = True - time_received_third_request = t - - has_sent_second_ack = False - time_sent_second_ack = rospy.Time() - has_sent_third_ack = False - time_has_sent_third_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_response']): - if (run_type == "SLF" and joiner_vehicle_number == 1): - if msg.m_header.recipient_id == leader_id and msg.m_header.sender_id == follower_id: - print("DEBUG: " + str(msg.m_header.sender_id) + " sending " + str(msg.m_header.recipient_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec()) + " with " + str(msg.is_accepted)) - if msg.plan_type.type == 6 and msg.is_accepted: - has_sent_second_ack = True - time_sent_second_ack = t - - elif (run_type == "ALF" and joiner_vehicle_number == 1): - if msg.m_header.recipient_id == leader_id and msg.m_header.sender_id == follower_id: - print("DEBUG: " + str(msg.m_header.sender_id) + " sending " + str(msg.m_header.recipient_id) + " plan_type " + str(msg.plan_type.type) + " at " + str(t.to_sec()) + " with " + str(msg.is_accepted)) - if msg.plan_type.type == 6 and msg.is_accepted: - has_sent_third_ack = True - time_sent_third_ack = t - - total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() - #print("DEBUG: Time between sending initial request and receiving ack: " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - #print("DEBUG: Time between sending second request and receiving ack: " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec") - #print("DEBUG: Time between sending initial request and receiving second ack: " + str(total_formation_duration) + " sec") - - if run_type == "SLR" or run_type == "SH-MID" or run_type == "SH-HIGH": - total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() - if joiner_vehicle_number == 2: - if has_received_initial_ack: - print("IHP2-1 Succeeded - Vehicle 2 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-1 Failed - Vehicle 2 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") - - if has_received_second_ack: - print("IHP2-3 Succeeded - Vehicle 2 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-3 Failed - Vehicle 2 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack] - - elif joiner_vehicle_number == 3: - if has_received_initial_ack: - print("IHP2-7 Succeeded - Vehicle 3 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-7 Failed - Vehicle 3 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") - - if has_received_second_ack: - print("IHP2-9 Succeeded - Vehicle 3 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-9 Failed - Vehicle 3 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-8 Succeeded: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-8 Failed: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack] - - elif run_type == "SLF": - if joiner_vehicle_number == 3: - total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() - if has_received_initial_ack: - print("IHP2-1 Succeeded - Vehicle 3 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-1 Failed - Vehicle 3 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") - - if has_received_second_ack: - print("IHP2-3 Succeeded - Vehicle 3 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-3 Failed - Vehicle 3 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-2 Succeeded: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-2 Succeeded: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack] - - elif joiner_vehicle_number == 1: - total_formation_duration = (time_sent_second_ack - time_sent_initial_request).to_sec() - if has_received_initial_ack: - print("IHP2-7 Succeeded - Vehicle 1 received initial ACK from leader for JOIN_PLATOON_FROM_FRONT at " + str(time_received_initial_ack.to_sec())+ " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-7 Failed - Vehicle 1 did not receive initial ACK from leader for JOIN_PLATOON_FROM_FRONT") - - if has_sent_second_ack: - print("IHP2-9 Succeeded - Vehicle 1 sent second ACK to leader for PLATOON_FRONT_JOIN and joins platoon at " +str(time_sent_second_ack.to_sec()) + " in " + str((time_sent_second_ack - time_received_second_request).to_sec()) + " sec; " + str((time_sent_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-9 Failed - Vehicle 1 did not send second ACK to leader for PLATOON_FRONT_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-8 Succeeded: Time between Vehicle 1 sending initial request and sending second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-8 Failed: Time between Vehicle 1 sending initial request and sending second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_sent_second_ack] - - elif run_type == "ALR": - if joiner_vehicle_number == 2: - total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() - if has_received_initial_ack: - print("IHP2-1 Succeeded - Vehicle 2 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-1 Failed - Vehicle 2 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") - - if has_received_second_ack: - print("IHP2-3 Succeeded - Vehicle 2 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " +str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-3 Failed - Vehicle 2 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack] - elif joiner_vehicle_number == 3: - total_formation_duration = (time_received_third_ack - time_sent_initial_request).to_sec() - if has_received_initial_ack: - print("IHP2-7 Succeeded (ALR): Vehicle 3 received initial ACK from leader for PLATOON_CUT_IN_JOIN at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-7 Failed (ALR): Vehicle 3 did not receive initial ACK from leader for PLATOON_CUT_IN_JOIN ") - - if has_received_second_ack: - print("IHP2-9 Succeeded (ALR): Vehicle 3 received second ACK from leader for CUT_IN_MID_OR_REAR_DONE at " +str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-9 Failed (ALR): Vehicle 3 did not receive second ACK from leader for CUT_IN_MID_OR_REAR_DONE") - - if has_received_third_ack: - print("IHP2-9.5 Succeeded (ALR): Vehicle 3 received third ACK from leader for PLATOON_FOLLOWER_JOIN at " + str(time_received_third_ack.to_sec()) + " in " + str((time_received_third_ack - time_sent_third_request).to_sec()) + " sec; " + str((time_received_third_ack-time_received_second_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-9.5 Failed (ALR): Vehicle 3 did not receive third ACK from leader for PLATOON_FOLLOWER_JOIN") - - if total_formation_duration <= 2.0: - print("IHP2-8 Succeeded (ALR): Time between Vehicle 3 sending initial request and receiving third ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-8 Succeeded (ALR): Time between Vehicle 3 sending initial request and receiving third ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack, time_received_third_ack] - - elif run_type == "ALF": - if joiner_vehicle_number == 3: - total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() - if has_received_initial_ack: - print("IHP2-1 Succeeded - Vehicle 3 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-1 Failed - Vehicle 3 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") - - if has_received_second_ack: - print("IHP2-3 Succeeded - Vehicle 3 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-3 Failed - Vehicle 3 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-2 Succeeded: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-2 Succeeded: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack] - elif joiner_vehicle_number == 1: - total_formation_duration = (time_sent_third_ack - time_sent_initial_request).to_sec() - if has_received_initial_ack: - print("IHP2-7 Succeeded (ALF): Vehicle 1 received initial ACK from leader for PLATOON_CUT_IN_JOIN at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-7 Failed (ALF): Vehicle 1 did not receive initial ACK from leader for PLATOON_CUT_IN_JOIN ") - - if has_received_second_ack: - print("IHP2-9 Succeeded (ALF): Vehicle 1 received second ACK from leader for CUT_IN_FRONT_DONE at " +str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-9 Failed (ALF): Vehicle 1 did not receive second ACK from leader for CUT_IN_FRONT_DONE ") - - if has_sent_third_ack: - print("IHP2-9.5 Succeeded (ALF): Vehicle 1 sent third ACK to leader for PLATOON_FRONT_JOIN at " + str(time_sent_third_ack.to_sec()) + " in " + str((time_sent_third_ack - time_received_third_request).to_sec()) + " sec; " + str((time_sent_third_ack-time_received_second_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-9.5 Failed (ALF): Vehicle 1 did not send third ACK to leader for PLATOON_FRONT_JOIN ") - - if total_formation_duration <= 2.0: - print("IHP2-8 Succeeded (ALF): Time between Vehicle 1 sending initial request and sending third ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-8 Succeeded (ALF): Time between Vehicle 1 sending initial request and sending third ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack, time_sent_third_ack] - -def received_tcm_analysis(bag, time_start_platooning, test_type, vehicle_number, expected_tcm_count): - - # List to store the receive times of each TCM msgnum [time_receive_msgnum1, time_receive_msgnum2, etc.] - tcm_receive_times = [None for i in range(0, expected_tcm_count)] - - # Store all transmitted TCR reqids so that only corresponding received TCMs are analyzed - tcr_reqids = [] - tcr_reqids_times = [] - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_geofence_request']): - tcr_reqids.append(msg.tcr_v01.reqid.id) - tcr_reqids_times.append(t) - - # Only analyze TCMs with reqids matching a transmitted TCR reqid. Populate the tcm_receive_times list - has_received_any_tcms = False - has_received_all_tcms = False - has_received_correct_tcms = True - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - reqid = msg.tcm_v01.reqid.id - if reqid in tcr_reqids: - has_received_any_tcms = True - msgnum = int(msg.tcm_v01.msgnum) - 1 - if tcm_receive_times[msgnum] is None: - time_received_after_platoon_start = (t-time_start_platooning).to_sec() - tcm_receive_times[msgnum] = time_received_after_platoon_start - - tcm_type = msg.tcm_v01.params.detail.choice - - if tcm_type == 12: - max_speed = msg.tcm_v01.params.detail.maxspeed - print("DEBUG: Received TCM msgnum " + str(msgnum) + " of " + str(msg.tcm_v01.msgtot) + " with type " + str(tcm_type) + " " + str(time_received_after_platoon_start) + " sec after platooning -- max speed " + str(max_speed)) - else: - print("Received non-maxspeed TCM") - - if None not in tcm_receive_times: - has_received_all_tcms = True - break - - if not has_received_any_tcms: - print("IHP2-4 Failed for vehicle " + str(vehicle_number) + ": No TCMs were received matching its TCR reqids") - print("IHP2-5 Failed for vehicle " + str(vehicle_number) + ": No TCMs were received matching its TCR reqids") - return - - #if has_received_all_tcms: - # time_received_first_tcm = min(tcm_receive_times) - # print("DEBUG: Vehicle " + str(vehicle_number) + " received first applicable TCM " + str(time_received_first_tcm) + " sec after beginning platooning") - - if min(tcm_receive_times) > 0: - print("IHP2-4 Succeeded for vehicle " + str(vehicle_number) + ": all TCMs were received after the vehicle began platooning") - else: - print("IHP2-4 Failed for vehicle " + str(vehicle_number) + ": at least one TCM was received before vehicle began platooning") - - if has_received_all_tcms and has_received_correct_tcms: - print("IHP2-5 Succeeded: CMV received all correct TCMs for test type " + str(test_type)) - elif not has_received_correct_tcms and not has_received_correct_tcms: - print("IHP2-5 Failed: CMV didn't receive all TCMs and didn't receive correct TCMs for test type " + str(test_type)) - elif not has_received_correct_tcms: - print("IHP2-5 Failed: CMV did not receive all correct TCMs for test type " + str(test_type)) - elif not has_received_all_tcms: - print("IHP2-5 Failed: CMV did not receive all TCM msgnums for test type " + str(test_type)) - - -def crosstrack_distance_analysis(join_vehicle_rosbag, other_vehicle_rosbag, run_type, join_vehicle_number, other_vehicle_number, time): - # Get other vehicle cross track at provided time (negative is to the left; positive is to the right) - for topic, msg, t in other_vehicle_rosbag.read_messages(topics=['/guidance/route_state'], start_time = time): - other_vehicle_cross_track = msg.cross_track - break - - # Get joining vehicle pose at provided time (negative is to the left; positive is to the right) - for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/guidance/route_state'], start_time = time): - join_vehicle_cross_track = msg.cross_track - break - - total_cross_track = abs(other_vehicle_cross_track - join_vehicle_cross_track) - print("DEBUG: Join vehicle " + str(join_vehicle_number) + " crosstrack " + str(join_vehicle_cross_track) + ", veh " + str(other_vehicle_number) + " crosstrack " + str(other_vehicle_cross_track) + ", total " + str(total_cross_track) + " meters at time " + str(time.to_sec())) - - if run_type == "ALR": - if total_cross_track < 1.5: - print("IHP2-20 Succeeded: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FOLLOWER_JOIN") - else: - print("IHP2-20 Failed: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FOLLOWER_JOIN") - elif run_type == "ALF": - if total_cross_track < 1.5: - print("IHP2-22 Succeeded: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FRONT_JOIN") - else: - print("IHP2-22 Failed: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FRONT_JOIN") - - return total_cross_track - -def downtrack_distance_analysis(join_vehicle_rosbag, other_vehicle_rosbag, run_type, join_vehicle_number, other_vehicle_number, time): - # Get other vehicle pose at provided time - for topic, msg, t in other_vehicle_rosbag.read_messages(topics=['/localization/current_pose'], start_time = time): - other_vehicle_pose = [msg.pose.position.x, msg.pose.position.y] - break - - # Get joining vehicle pose at provided time - for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/localization/current_pose'], start_time = time): - join_vehicle_pose = [msg.pose.position.x, msg.pose.position.y] - break - - # Get joining vehicle speed - for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time): - join_vehicle_speed = msg.twist.linear.x - break - - distance_between_vehicles = ((other_vehicle_pose[0] - join_vehicle_pose[0])**2 + (other_vehicle_pose[1] - join_vehicle_pose[1])**2) ** 0.5 - time_gap_between_vehicles = distance_between_vehicles / join_vehicle_speed - - print("DEBUG: Joining vehicle " + str(join_vehicle_number) + " speed is " + str(join_vehicle_speed) + " m/s") - print("DEBUG: Distance between joining vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " at time " + str(time.to_sec()) + " is " + str(distance_between_vehicles) + " meters") - print("DEBUG: Time Gap between joining vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " at time " + str(time.to_sec()) + " is " + str(time_gap_between_vehicles) + " seconds") - - if run_type == "SLR": - # Values are the same for same-lane front join and same-lane rear join - if distance_between_vehicles <= 90.0 or time_gap_between_vehicles <= 15.0: - print("IHP2-17 (SLR) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - else: - print("IHP2-17 (SLR) Failed: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - - elif run_type == "SH-MID" or run_type == "SH-HIGH": - # This test is for a same-lane rear join - if distance_between_vehicles <= 90.0 or time_gap_between_vehicles <= 15.0: - print("IHP2-17 (SH-MID or SH-HIGH) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - else: - print("IHP2-17 (SH-MID or SH-HIGH) Failed: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - - elif run_type == "SLF": - # Values are the same for same-lane front join and same-lane rear join - if distance_between_vehicles <= 90.0 or time_gap_between_vehicles <= 15.0: - print("IHP2-18 (SLF) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_FRONT") - else: - print("IHP2-18 (SLF) Failed: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_FRONT") - - elif run_type == "ALR": - if join_vehicle_number == 3: - # No time gap thresholds for adjacent lane rear join vehicle - if distance_between_vehicles <= 100.0: - print("IHP2-19 (SLF) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and platoon rear vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=100 desired) at PLATOON_CUT_IN_JOIN") - else: - print("IHP2-19 (SLF) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and platoon rear vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=100 desired) at PLATOON_CUT_IN_JOIN") - elif join_vehicle_number == 2: - if distance_between_vehicles <= 90.0 or time_gap_between_vehicles <= 15.0: - print("IHP2-17/18 (SLR in ALR) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - else: - print("IHP2-17/18 (SLR in ALR) Failed: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - - elif run_type == "ALF": - if join_vehicle_number == 1: - # No time gap thresholds for adjacent lane front join vehicle - if distance_between_vehicles <= 150.0: - print("IHP2-21 (ALF) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and platoon lead vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=150 desired) at PLATOON_CUT_IN_JOIN") - else: - print("IHP2-21 (ALF) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and platoon lead vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=150 desired) at PLATOON_CUT_IN_JOIN") - elif join_vehicle_number == 3: - if distance_between_vehicles <= 90.0 or time_gap_between_vehicles <= 15.0: - print("IHP2-17/18 (SLR in ALF) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - else: - print("IHP2-17/18 (SLR in ALF) Failed: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - - return distance_between_vehicles - -# Main Function; run all tests from here -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - source_folder = sys.argv[1] - - # # Re-direct the output of print() to a specified .txt file: - # orig_stdout = sys.stdout - # current_time = datetime.datetime.now() - # text_log_filename = "Results_" + str(current_time) + ".txt" - # text_log_file_writer = open(text_log_filename, 'w') - # sys.stdout = text_log_file_writer - - # # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - # csv_results_filename = "Results_" + str(current_time) + ".csv" - # csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - # csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", - # "IHP2-1", "IHP2-2", "IHP2-3", "IHP2-4", "IHP2-5", - # "IHP2-6", "IHP2-7", "IHP2-8", "IHP2-9", "IHP2-10", - # "IHP2-11", "IHP2-12", "IHP2-13", "IHP2-14", "IHP2-15", - # "IHP2-16", "IHP2-17", "IHP2-18", "IHP2-19"]) - - # Constants - BLUE_LEXUS = "DOT-45244" - BLACK_PACIFICA = "DOT-45245" - WHITE_PACIFICA = "DOT-45246" - - # SLR Test 5 removed (BP didn't engage), Test 6 removed (incorrect BL rosbag) - slr_bag_files = [[["SLR_BL_1_2022-07-13-19-43-10.bag", - "SLR_WP_1_2022-07-13-19-43-51.bag", - "SLR_BP_01.bag"], - [BLUE_LEXUS, WHITE_PACIFICA, BLACK_PACIFICA]], - [["SLR_BL_2_2022-07-13-19-57-25.bag", - "SLR_WP_2_2022-07-13-19-57-54.bag", - "SLR_BP_02.bag"], - [BLUE_LEXUS, WHITE_PACIFICA, BLACK_PACIFICA]], - [["SLR_BP_3_2022-07-13-20-24-52.bag", - "SLR_WP_3_2022-07-13-20-24-27.bag", - "SLR_BL_3_2022-07-13-20-24-30.bag"], - [BLACK_PACIFICA, WHITE_PACIFICA, BLUE_LEXUS]], - [["SLR_BP_4_2022-07-13-20-32-58.bag", - "SLR_WP_4_2022-07-13-20-34-47.bag", - "SLR_BL_4_2022-07-13-20-34-41.bag"], - [BLACK_PACIFICA, WHITE_PACIFICA, BLUE_LEXUS]]] - - slf_bag_files = [[["SLF_BL_1_2022-07-14-13-23-46.bag", - "SLF_BP_01_2022-07-14-13-27-17.bag", - "SLF_WP_1_2022-07-14-13-26-23.bag"], - [BLUE_LEXUS, BLACK_PACIFICA, WHITE_PACIFICA]], - [["SLF_BL_2_2022-07-14-13-32-51.bag", - "SLF_BP_2_2022-07-14-13-32-23.bag", - "SLF_WP_2_2022-07-14-13-32-08.bag"], - [BLUE_LEXUS, BLACK_PACIFICA, WHITE_PACIFICA]], - [["SLF_BL_3_2022-07-14-13-37-47.bag", - "SLF_WP_3_2022-07-14-13-37-49.bag", - "SLF_BP_3_2022-07-14-13-37-53.bag"], - [BLUE_LEXUS, WHITE_PACIFICA, BLACK_PACIFICA]], - [["SLF_BP_4_2022-07-14-13-46-28.bag", - "SLF_WP_4_2022-07-14-13-46-17.bag", - "SLF_BL_4_2022-07-14-13-46-30.bag"], - [BLACK_PACIFICA, WHITE_PACIFICA, BLUE_LEXUS]], - [["SLF_WP_5_2022-07-14-13-53-29.bag", - "SLF_BP_5_2022-07-14-13-53-37.bag", - "SLF_BL_5_2022-07-14-13-53-08.bag"], - [WHITE_PACIFICA, BLACK_PACIFICA, BLUE_LEXUS]], - [["SLF_WP_6_2022-07-14-13-59-05.bag", - "SLF_BP_6_2022-07-14-13-58-35.bag", - "SLF_BL_6_2022-07-14-13-57-48.bag"], - [WHITE_PACIFICA, BLACK_PACIFICA, BLUE_LEXUS]]] - - alr_bag_files = [[["ALR2_WP_1_2022-07-14-18-21-10.bag", - "ALR2_BL_1_2022-07-14-18-25-00.bag", - "ALR2_BP_1_2022-07-14-18-19-30.bag"], - [WHITE_PACIFICA, BLUE_LEXUS, BLACK_PACIFICA]], - [["ALR2_WP_2_2022-07-14-18-31-06.bag", - "ALR2_BL_2_2022-07-14-18-30-53.bag", - "ALR2_BP_2_2022-07-14-18-31-31.bag"], - [WHITE_PACIFICA, BLUE_LEXUS, BLACK_PACIFICA]], - [["ALR2_WP_3_2022-07-14-18-36-16.bag", - "ALR2_BL_3_2022-07-14-18-36-43.bag", - "ALR2_BP_3_2022-07-14-18-36-40.bag"], - [WHITE_PACIFICA, BLUE_LEXUS, BLACK_PACIFICA]], - [["ALR2_BP_4_2022-07-14-18-45-55.bag", - "ALR2_WP_4_2022-07-14-18-45-03.bag", - "ALR2_BL_4_2022-07-14-18-46-06.bag"], - [BLACK_PACIFICA, WHITE_PACIFICA, BLUE_LEXUS]], - [["ALR2_BP_5_2022-07-14-18-54-27.bag", - "ALR2_WP_5_2022-07-14-18-53-32.bag", - "ALR2_BL_5_2022-07-14-18-54-23.bag"], - [BLACK_PACIFICA, WHITE_PACIFICA, BLUE_LEXUS]]] - - # Note: Test 3 data was removes since it could not be processed - alf_bag_files = [[["ALF_BP_1_2022-07-14-19-04-30.bag", - "ALF_BL_1_2022-07-14-19-07-10.bag", - "ALF_WP_1_2022-07-14-19-07-16.bag"], - [BLACK_PACIFICA, BLUE_LEXUS, WHITE_PACIFICA]], - [["ALF_BP_2_2022-07-14-19-13-35.bag", - "ALF_BL_2_2022-07-14-19-13-14.bag", - "ALF_WP_2_2022-07-14-19-13-17.bag"], - [BLACK_PACIFICA, BLUE_LEXUS, WHITE_PACIFICA]], - [["ALF_BL_4_2022-07-14-19-35-27.bag", - "ALF_WP_4_2022-07-14-19-35-12.bag", - "ALF_BP_4_2022-07-14-19-35-26.bag"], - [BLUE_LEXUS, WHITE_PACIFICA, BLACK_PACIFICA]], - [["ALF_BL_5_2022-07-14-19-42-11.bag", - "ALF_BP_5_2022-07-14-19-41-13.bag", - "ALF_WP_5_2022-07-14-19-41-35.bag"], - [BLUE_LEXUS, BLACK_PACIFICA, WHITE_PACIFICA]]] - - sh_mid_bag_files = [[["SH_BP_01.bag", - "SH_WP_1_2022-07-13.bag"], - [BLACK_PACIFICA, WHITE_PACIFICA]]] - - # Test 2 failed; Not Black Pacifica bag for Test 7 - sh_high_bag_files = [[["SH_BP_03.bag", - "SH_WP_3_2022-07-13-16-06-49.bag"], - [BLACK_PACIFICA, WHITE_PACIFICA]]] - - # Combine each grouping of test case data files - ihp2_bag_files = slr_bag_files + slf_bag_files + alr_bag_files + alf_bag_files + sh_mid_bag_files + sh_high_bag_files - - # Loop to conduct data analysis on each grouping of test run rosbags: - test_num = 0 - for test_case_files in ihp2_bag_files: - test_num += 1 - print("*****************************************************************") - if test_case_files in slr_bag_files: - test_type = "SLR" - print("IHP2 - SLR Test Case") - elif test_case_files in slf_bag_files: - test_type = "SLF" - print("IHP2 - SLF Test Case") - elif test_case_files in alr_bag_files: - test_type = "ALR" - print("IHP2 - ALR Test Case") - elif test_case_files in alf_bag_files: - test_type = "ALF" - print("IHP2 - ALF Test Case") - elif test_case_files in sh_mid_bag_files: - test_type = "SH-MID" - print("IHP2 - SH MID Test Case") - elif test_case_files in sh_high_bag_files: - test_type = "SH-HIGH" - print("IHP2 - SH High Test Case") - else: - print("Unknown test run type being processed.") - continue - - # Store the rosbag and vehicle type for each vehicle in the lineup (Vehicle 1, Vehicle 2, Vehicle 3) - bag_files_list = test_case_files[0] - vehicle_types = test_case_files[1] - for file_number in range(0, len(bag_files_list)): - print("--") - bag_file = bag_files_list[file_number] - if file_number == 0: - vehicle_number = 1 - vehicle_1_rosbag_name = bag_files_list[file_number] - vehicle_1_type = vehicle_types[0] - elif file_number == 1: - vehicle_number = 2 - vehicle_2_rosbag_name = bag_files_list[file_number] - vehicle_2_type = vehicle_types[1] - elif file_number == 2: - vehicle_number = 3 - vehicle_3_rosbag_name = bag_files_list[file_number] - vehicle_3_type = vehicle_types[2] - - print("Processing new bag for vehicle " + str(file_number+1) + ": " + str(bag_file) + " for " + str(vehicle_types[file_number])) - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - # sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(ihp2_bag_files.index(test_case_files) + 1) + " of " + str(len(ihp2_bag_files)) + ")") - # sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - - if vehicle_number == 1: - vehicle_1_rosbag = rosbag.Bag(bag_file_path) - elif vehicle_number == 2: - vehicle_2_rosbag = rosbag.Bag(bag_file_path) - elif vehicle_number == 3: - vehicle_3_rosbag = rosbag.Bag(bag_file_path) - - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test - print("Getting engagement times at " + str(datetime.datetime.now())) - if vehicle_number == 1: - time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_1_rosbag) - time_1_start_engagement = time_start_engagement - time_1_end_engagement = time_end_engagement - if vehicle_number == 2: - time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_2_rosbag) - time_2_start_engagement = time_start_engagement - time_2_end_engagement = time_end_engagement - if vehicle_number == 3: - time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_3_rosbag) - time_3_start_engagement = time_start_engagement - time_3_end_engagement = time_end_engagement - print("Got engagement times at " + str(datetime.datetime.now())) - - if (not found_test_times): - print("Could not find test case engagement start and end times in bag file.") - #continue - else: - print("Began engagement at " + str(time_start_engagement.to_sec())) - print("Ended engagement at " + str(time_end_engagement.to_sec())) - print("Time spent engaged: " + str((time_end_engagement - time_start_engagement).to_sec()) + " seconds") - - if vehicle_number == 1: - speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_1_rosbag) - veh_1_speed_limit_zone_change_times = speed_limit_zone_change_times - #generate_speed_plot(vehicle_1_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) - #generate_crosstrack_plot(vehicle_1_rosbag, time_start_engagement, time_end_engagement, bag_file) - if vehicle_number == 2: - speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_2_rosbag) - #generate_speed_plot(vehicle_2_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) - #generate_crosstrack_plot(vehicle_2_rosbag, time_start_engagement, time_end_engagement, bag_file) - if vehicle_number == 3: - speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_3_rosbag) - #generate_speed_plot(vehicle_3_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) - #generate_crosstrack_plot(vehicle_3_rosbag, time_start_engagement, time_end_engagement, bag_file) - - if test_type == "SLR": - # Vehicle 2 joins Vehicle 1 from rear; then Vehicle 3 joins 1/2 from Rear - - # Platoon Formation - Joiner Perspective (IHP2 metrics 1,2,3,7,8,9) - vehicle_2_platooning_times = joiner_platoon_formation_analysis(vehicle_2_rosbag, test_type, vehicle_1_type, vehicle_2_type, 2) - vehicle_3_platooning_times = joiner_platoon_formation_analysis(vehicle_3_rosbag, test_type, vehicle_1_type, vehicle_3_type, 3) - time_vehicle_1_platooning_start = vehicle_2_platooning_times[1] - - print("DEBUG: Vehicle 1 Platoon Start Time: " + str(time_vehicle_1_platooning_start.to_sec())) - print("DEBUG: Vehicle 2 Platoon Start Time: " + str(vehicle_2_platooning_times[1].to_sec())) - print("DEBUG: Vehicle 3 Platoon Start Time: " + str(vehicle_3_platooning_times[1].to_sec())) - - received_tcm_analysis(vehicle_1_rosbag, time_vehicle_1_platooning_start, test_type, 1, expected_tcm_count=7) - received_tcm_analysis(vehicle_2_rosbag, vehicle_2_platooning_times[1], test_type, 2, expected_tcm_count=7) - received_tcm_analysis(vehicle_3_rosbag, vehicle_3_platooning_times[1], test_type, 3, expected_tcm_count=7) - - print("IHP2-6 N/A for SLR Test; no platoon-specific TCMs") - print("IHP2-10 N/A for SLR Test; TCM speed limit matches original speed limit") - print("IHP2-11 N/A for SLR Test; no headway-specific TCMs") - print("IHP2-12 N/A for SLR Test; TCM speed limit matches original speed limit") - print("IHP2-13 N/A for SLR Test; no headway-specific TCMs") - - downtrack_distance_analysis(vehicle_2_rosbag, vehicle_1_rosbag, test_type, 2, 1, vehicle_2_platooning_times[0]) - downtrack_distance_analysis(vehicle_3_rosbag, vehicle_2_rosbag, test_type, 3, 2, vehicle_3_platooning_times[0]) - - #generate_platooning_plot(vehicle_2_rosbag, vehicle_2_platooning_times[1], vehicle_2_rosbag_name, test_type, 2) - #generate_platooning_plot(vehicle_3_rosbag, vehicle_3_platooning_times[1], vehicle_3_rosbag_name, test_type, 3) - - #generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_1_start_engagement, time_1_end_engagement, test_type, test_num) - - elif test_type == "SLF": - # Vehicle 3 joins Vehicle 2 from rear; then Vehicle 1 joins 2/3 from Front - - vehicle_1_platooning_times = joiner_platoon_formation_analysis(vehicle_1_rosbag, test_type, vehicle_2_type, vehicle_1_type, 1) - vehicle_3_platooning_times = joiner_platoon_formation_analysis(vehicle_3_rosbag, test_type, vehicle_2_type, vehicle_3_type, 3) - - downtrack_distance_analysis(vehicle_1_rosbag, vehicle_2_rosbag, test_type, 1, 2, vehicle_1_platooning_times[0]) - downtrack_distance_analysis(vehicle_3_rosbag, vehicle_2_rosbag, test_type, 3, 2, vehicle_3_platooning_times[0]) - - #generate_platooning_plot(vehicle_3_rosbag, vehicle_3_platooning_times[1], vehicle_3_rosbag_name, test_type, 3) - #generate_platooning_plot(vehicle_2_rosbag, vehicle_1_platooning_times[1], vehicle_2_rosbag_name, test_type, 2) - - #generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_2_start_engagement, time_2_end_engagement, test_type, test_num) - - elif test_type == "ALR": - # Vehicle 2 joins Vehicle 1 from rear; then Vehicle 3 joins 1/2 from adjacent rear - - vehicle_2_platooning_times = joiner_platoon_formation_analysis(vehicle_2_rosbag, test_type, vehicle_1_type, vehicle_2_type, 2) - vehicle_3_platooning_times = joiner_platoon_formation_analysis(vehicle_3_rosbag, test_type, vehicle_1_type, vehicle_3_type, 3) - - downtrack_distance_analysis(vehicle_2_rosbag, vehicle_1_rosbag, test_type, 2, 1, vehicle_2_platooning_times[0]) - - downtrack_distance_analysis(vehicle_3_rosbag, vehicle_2_rosbag, test_type, 3, 2, vehicle_3_platooning_times[0]) - crosstrack_distance_analysis(vehicle_3_rosbag, vehicle_2_rosbag, test_type, 3, 2, vehicle_3_platooning_times[2]) - - #generate_platooning_plot(vehicle_3_rosbag, vehicle_3_platooning_times[2], vehicle_3_rosbag_name, test_type, 3) - #generate_platooning_plot(vehicle_2_rosbag, vehicle_2_platooning_times[1], vehicle_2_rosbag_name, test_type, 2) - - #generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_1_start_engagement, time_1_end_engagement, test_type, test_num) - - elif test_type == "ALF": - # Vehicle 3 joins Vehicle 2 from rear; then Vehicle 1 joins 2/3 from adjacent front - - vehicle_3_platooning_times = joiner_platoon_formation_analysis(vehicle_3_rosbag, test_type, vehicle_2_type, vehicle_3_type, 3) - vehicle_1_platooning_times = joiner_platoon_formation_analysis(vehicle_1_rosbag, test_type, vehicle_2_type, vehicle_1_type, 1) - - downtrack_distance_analysis(vehicle_3_rosbag, vehicle_2_rosbag, test_type, 3, 2, vehicle_3_platooning_times[0]) - - downtrack_distance_analysis(vehicle_1_rosbag, vehicle_2_rosbag, test_type, 1, 2, vehicle_1_platooning_times[0]) - crosstrack_distance_analysis(vehicle_1_rosbag, vehicle_2_rosbag, test_type, 1, 2, vehicle_1_platooning_times[2]) - - #generate_platooning_plot(vehicle_3_rosbag, vehicle_3_platooning_times[1], vehicle_3_rosbag_name, test_type, 3) - #generate_platooning_plot(vehicle_2_rosbag, vehicle_1_platooning_times[2], vehicle_2_rosbag_name, test_type, 2) - - #generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_2_start_engagement, time_2_end_engagement, test_type, test_num) - - elif test_type == "SH-MID": - # Vehicle 2 joins vehicle 1 from rear - - vehicle_2_platooning_times = joiner_platoon_formation_analysis(vehicle_2_rosbag, test_type, vehicle_1_type, vehicle_2_type, 2) - downtrack_distance_analysis(vehicle_2_rosbag, vehicle_1_rosbag, test_type, 2, 1, vehicle_2_platooning_times[0]) - received_tcm_analysis(vehicle_1_rosbag, vehicle_2_platooning_times[1], test_type, 1, expected_tcm_count=10) - received_tcm_analysis(vehicle_2_rosbag, vehicle_2_platooning_times[1], test_type, 2, expected_tcm_count=10) - - #generate_platooning_plot(vehicle_2_rosbag, vehicle_2_platooning_times[1], vehicle_2_rosbag_name, test_type, 2) - #generate_two_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, time_1_start_engagement, time_1_end_engagement, veh_1_speed_limit_zone_change_times, test_type, test_num) - - elif test_type == "SH-HIGH": - # Vehicle 2 joins vehicle 1 from rear - - vehicle_2_platooning_times = joiner_platoon_formation_analysis(vehicle_2_rosbag, test_type, vehicle_1_type, vehicle_2_type, 2) - downtrack_distance_analysis(vehicle_2_rosbag, vehicle_1_rosbag, test_type, 2, 1, vehicle_2_platooning_times[0]) - received_tcm_analysis(vehicle_1_rosbag, vehicle_2_platooning_times[1], test_type, 1, expected_tcm_count=10) - received_tcm_analysis(vehicle_2_rosbag, vehicle_2_platooning_times[1], test_type, 2, expected_tcm_count=10) - - #generate_platooning_plot(vehicle_2_rosbag, vehicle_2_platooning_times[1], vehicle_2_rosbag_name, test_type, 2) - #generate_two_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, time_1_start_engagement, time_1_end_engagement, veh_1_speed_limit_zone_change_times, test_type, test_num) - - print("Analysis complete") - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_port_drayage_rosbags.py b/engineering_tools/data_processing/analyze_port_drayage_rosbags.py deleted file mode 100644 index c24aecbebc..0000000000 --- a/engineering_tools/data_processing/analyze_port_drayage_rosbags.py +++ /dev/null @@ -1,975 +0,0 @@ -#!/usr/bin/python3 - -# Copyright (C) 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. - -from inspect import TPFLAGS_IS_ABSTRACT -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime -import math - -# HOW TO USE SCRIPT: -# Run the following in a terminal to download dependencies: -# sudo add-apt-repository ppa:deadsnakes/ppa -# sudo apt-get update -# sudo apt install python3.7 -# python3.7 -m pip install --upgrade pip -# python3.7 -m pip install matplotlib -# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg -# python3.7 -m pip install lz4 -# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ -# In terminal, navigate to the directory that contains this python script and run the following: -# python3.7 analyze_port_drayage_rosbags.py - -def generate_speed_plot(bag, engagement_times, route_id): - # Speed command: /hardware_interface/arbitrated_speed_commands: msg.speed - # True Speed: /hardware_interface/pacmod_parsed_tx/vehicle_speed_rpt: msg.vehicle_speed - total_duration = 25 - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - time_duration = rospy.Duration(total_duration) - - # Get the true vehicle speed and plot it - first = True - true_vehicle_speed_times = [] - true_vehicle_speeds = [] - # Note: This topic name assumes a pacmod controller is being used (freightliners or lexus) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/pacmod/parsed_tx/vehicle_speed_rpt'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - true_vehicle_speed_times.append((t-time_start).to_sec()) - true_vehicle_speeds.append(msg.vehicle_speed) - - first = True - cmd_vehicle_speed_times = [] - cmd_vehicle_speeds = [] - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/arbitrated_speed_commands'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - cmd_vehicle_speed_times.append((t-time_start).to_sec()) - cmd_vehicle_speeds.append(msg.speed) - - fig, ax = plt.subplots() - print(true_vehicle_speed_times[0]) - ax.plot(true_vehicle_speed_times, true_vehicle_speeds, 'b--', label='Actual Speed') - ax.plot(cmd_vehicle_speed_times, cmd_vehicle_speeds, 'g:', label='Cmd Speed') - ax.legend() - ax.set_title("Speed (Cmd and Actual) for Route " + str(route_id)) - ax.set_xlabel("Time (seconds) since start of engagement") - ax.set_ylabel("Vehicle Speed (m/s)") - plt.show() - - return - -def generate_steering_plot(bag, engagement_times, route_id): - # Speed command: /hardware_interface/arbitrated_speed_commands: msg.speed - # True Speed: /hardware_interface/pacmod_parsed_tx/vehicle_speed_rpt: msg.vehicle_speed - total_duration = 25 - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - time_duration = rospy.Duration(total_duration) - - # Get the true vehicle speed and plot it - first = True - true_steering_times = [] - true_steering = [] - cmd_steering_times = [] - cmd_steering = [] - # Note: This topic name assumes a pacmod controller is being used (freightliners or lexus) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/pacmod/parsed_tx/steer_rpt'], start_time = time_start_engagement, end_time = time_end_engagement): #time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - true_steering_times.append((t-time_start).to_sec()) - true_steering.append(msg.output) - cmd_steering_times.append((t-time_start).to_sec()) - cmd_steering.append(msg.command) - - - fig, ax = plt.subplots() - ax.plot(true_steering_times, true_steering, 'b--', label='Actual Steering (rad)') - ax.plot(cmd_steering_times, cmd_steering, 'g:', label='Cmd Steering (rad)') - ax.legend() - ax.set_title("Steering Angle (Cmd and Actual) for Route " + str(route_id)) - ax.set_xlabel("Time (seconds) since start of engagement") - ax.set_ylabel("Steering Angle (rad)") - plt.show() - - return - -# Helper Function: Get the original speed limit for the lanelets within the vehicle's route -# Note: Assumes that all lanelets in the route share the same speed limit prior to the first geofence CARMA Cloud message being processed. -def get_route_original_speed_limit(bag, time_test_start_engagement): - # Initialize the return variable - original_speed_limit = 0.0 - - # Find the speed limit associated with the first lanelet when the system first becomes engaged - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_test_start_engagement): - original_speed_limit = msg.speed_limit - break - - return original_speed_limit - -# Helper Function: Obtain the start and end times of each 'System Engaged' portion of the provided bag file. -# Function returns all start times and corresponding end times in separate lists. -def get_test_case_engagement_times(bag, num_engagement_sections): - # Initialize system engagement start and end times - engagement_start_times = [] - engagement_end_times = [] - - # Loop through /guidance/state messages to determine each start and end time of engagement - is_engaged = False - for topic, msg, t in bag.read_messages(topics=['/guidance/state']): - # If entering engagement, track this start time - if (msg.state == 4 and not is_engaged): - time_start_engagement = t - is_engaged = True - - # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance - # state can be published. - if (msg.state == 4): - time_last_engaged = t - else: - if (is_engaged): - engagement_start_times.append(time_start_engagement) - engagement_end_times.append(time_last_engaged) - is_engaged = False - - found_engagement_times = True - if (len(engagement_start_times) != num_engagement_sections): - print("Error: Bag file has " + str(len(engagement_start_times)) + " engagement start times, not " + str(num_engagement_sections)) - found_engagement_times = False - else: - print("Success: Bag file has " + str(len(engagement_start_times)) + " engagement start times, expected " + str(num_engagement_sections)) - if (len(engagement_end_times) != num_engagement_sections): - print("Error: Bag file has " + str(len(engagement_end_times)) + " engagement end times, not " + str(num_engagement_sections)) - found_engagement_times = False - else: - print("Success: Bag file has " + str(len(engagement_end_times)) + " engagement end times, expected " + str(num_engagement_sections)) - - return engagement_start_times, engagement_end_times, found_engagement_times - -# Helper Function: Print out the times associated with the vehicle entering each new lanelet according to /guidance/route_state -def print_lanelet_entrance_times(bag, time_start_engagement): - # Print out time vehicle enters each lanelet according to /guidance/route_state - id = 0 - print("/guidance/route_state lanelet change times:") - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): - if msg.lanelet_id != id: - print("Time: " + str(t.to_sec()) + "; Lanelet: " + str(msg.lanelet_id) + "; Speed Limit: " + str(msg.speed_limit)) - id = msg.lanelet_id - - return - -########################################################################################################### -# Port Drayage 1: Vehicle achieves its target speed (+/- 2 mph of speed limit). -########################################################################################################### -def check_vehicle_reaches_steady_state(bag, engagement_times, speed_limit_ms): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - # (m/s) Threshold offset of vehicle speed to speed limit to be considered at steady state - threshold_speed_limit_offset = 0.89408 # 0.89408 m/s is 2 mph - min_steady_state_speed = speed_limit_ms - threshold_speed_limit_offset - max_steady_state_speed = speed_limit_ms + threshold_speed_limit_offset - - # Get the time that the vehicle reaches within the set offset of the speed limit (while system is engaged) - has_reached_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement, end_time = time_end_engagement): - current_speed = msg.twist.linear.x # Current vehicle speed in m/s - if (max_steady_state_speed >= current_speed >= min_steady_state_speed): - has_reached_steady_state = True - break - - if has_reached_steady_state: - print("PD-1-" + str(engagement_times[2]) + " Succeeded; vehicle reached +/- 0.89408 m/s (2 mph) of speed limit " + str(speed_limit_ms) + " m/s.") - is_successful = True - else: - print("PD-1-" + str(engagement_times[2]) + " Failed; vehicle did not reach speed within +/- 0.89408 m/s (2 mph) of speed limit " + str(speed_limit_ms) + " m/s.") - is_successful = False - - return is_successful - -########################################################################################################### -# Port Drayage 4: The speed limit while not inside the Port or Staging Area is 20-25 mph. (8.94-11.176 m/s). -########################################################################################################### -def check_speed_limit_outside_port_and_staging_area(bag, engagement_times, lanelet_ids, min_speed_limit_ms, max_speed_limit_ms): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - # Loop through /route_state topic to get the current speed limit for a given timestamp - encountered_lanelet_ids_in_speed_limit_range = [] - id = 0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_end_engagement): - - # If a new lanelet ID is encountered, check that it is within the correct speed limit range and add it to list - if msg.lanelet_id != id: - id = msg.lanelet_id - if (min_speed_limit_ms <= msg.speed_limit <= max_speed_limit_ms): - encountered_lanelet_ids_in_speed_limit_range.append(id) - - # Check list of enocuntered lanelet IDs that satisfy speed limit range and check against expected list - is_successful = False - if encountered_lanelet_ids_in_speed_limit_range == lanelet_ids: - print("PD-4-" + str(engagement_times[2]) + " Succeeded; all lanelets outside of Staging Area and Port had a speed limit between 8.94-11.18 m/s (20-25mph).") - is_successful = True - else: - print("PD-4-" + str(engagement_times[2]) + " Failed; not all lanelets outside of Staging Area and Port had a speed limit between 8.94-11.18 m/s (20-25mph).") - is_successful = False - - return is_successful - -########################################################################################################### -# Port Drayage 19: The speed limit while inside the Port or Staging Area is 10 mph. (4.47 m/s). -########################################################################################################### -def check_speed_limit_inside_port_and_staging_area(bag, engagement_times, target_speed_limit_ms): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - # Set acceptable speed limit range - speed_limit_threshold_ms = 0.223 # Threshold (in m/s) the actual speed limit can be from the target speed limit - min_speed_limit_ms = target_speed_limit_ms - speed_limit_threshold_ms - max_speed_limit_ms = target_speed_limit_ms + speed_limit_threshold_ms - - # Loop through /route_state topic to get the current speed limit for a given timestamp - id = 0 - is_successful = True - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_end_engagement): - # If a lanelet is encountered with a speed limit outside the target range, this metric fails - if not (min_speed_limit_ms <= msg.speed_limit <= max_speed_limit_ms): - is_successful = False - id = msg.lanelet_id - print("PD-19-" + str(engagement_times[2]) + " Failed: Lanelet " + str(id) + " had speed limit of " + str(msg.speed_limit) + " m/s") - break - - if is_successful: - print("PD-19-" + str(engagement_times[2]) + " Succeeded; all speed limits were " + str(target_speed_limit_ms) + " m/s") - - return is_successful - -########################################################################################################### -# Port Drayage 23: After the CMV is engaged on the route to its next destination, the CMV's actual trajectory -# will include an acceleration section. The average acceleration over the entire section -# shall be no less than 1 m/s^2, and the average acceleration over any 1-second portion -# of the section shall be no greater than 2.0 m/s^2. -########################################################################################################### -def check_acceleration_after_engagement(bag, engagement_times, target_speed_limit_ms): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - # Set target speed (an offset below the target speed limit, since acceleration rate will decrease after this point) - speed_limit_offset_ms = 0.89 # 0.89 m/s is 2 mph - target_speed_ms = target_speed_limit_ms - speed_limit_offset_ms - - # Obtain timestamp associated with the start of the acceleration section - time_start_accel = rospy.Time() - speed_start_accel_ms = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement): - if msg.speed >= 0.1: - speed_start_accel_ms = msg.speed * 0.277778 # Conversion kph to m/s - time_start_accel = t - break - - # Obtain timestamp associated with the end of the acceleration section - # Note: This is either the first deceleration after the start of the accel section or the moment - # when the vehicle's speed reaches some offset of the speed limit. - prev_speed = 0.0 - first = True - time_end_accel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_accel): - if first: - prev_speed = msg.speed - first = False - continue - - delta_speed = msg.speed - prev_speed - current_speed_ms = msg.speed * 0.27777 # Conversion of kph to m/s - #if delta_speed <= 0 or (msg.speed*0.2777 >= target_speed_ms): - if (msg.speed*0.2777 >= target_speed_ms): - time_end_accel = t - speed_end_accel_ms = current_speed_ms - speed_end_accel_mph = speed_end_accel_ms * 2.2369 - #print("Speed at end of acceleration section: " + str(speed_end_accel_mph) + " mph") - break - - prev_speed = msg.speed - - # Check the total average acceleration rate - total_avg_accel = (speed_end_accel_ms - speed_start_accel_ms) / (time_end_accel-time_start_accel).to_sec() - #print("Total average accel: " + str(total_avg_accel) + " m/s^2") - - # Check the 1-second window averages - # Obtain average decelation over all 1-second windows in acceleration section - one_second_duration = rospy.Duration(1.0) - all_one_second_windows_successful = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_accel, end_time = (time_end_accel-one_second_duration)): - speed_initial_ms = msg.speed * 0.277777 # Conversion from kph to m/s - time_initial = t - - speed_final_ms = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): - speed_final_ms = msg.speed * 0.277777 # Conversion from kph to m/s - t_final = t - break - - one_second_accel = (speed_final_ms - speed_initial_ms) / (t_final - time_initial).to_sec() - - if one_second_accel > 2.0: - print("Failure: 1-second window accel rate was " + str(one_second_accel) + " m/s^2; end time was " + str((t_final - t_final).to_sec()) + " seconds after start of engagement.") - all_one_second_windows_successful = False - #else: - # print("Success: 1-second window has accel rate at " + str(one_second_decel) + " m/s^2") - - # Print success/failure statements - total_deceleration_rate_successful = False - if total_avg_accel >= 1.0: - print("PD-23-" + str(engagement_times[2]) + " (avg accel at start) Succeeded; total average acceleration was " + str(total_avg_accel) + " m/s^2") - total_deceleration_rate_successful = True - else: - print("PD-23-" + str(engagement_times[2]) + " (avg accel at start) Failed; total average acceleration was " + str(total_avg_accel) + " m/s^2") - - if all_one_second_windows_successful: - print("PD-23-" + str(engagement_times[2]) + " (1-second accel after start) Succeeded; no occurrences of 1-second average acceleration above 2.0 m/s^2") - else: - print("PD-23-" + str(engagement_times[2]) + " (1-second accel after start) Failed; at least one occurrence of 1-second average acceleration above 2.0 m/s^2") - - is_successful = False - if total_deceleration_rate_successful and all_one_second_windows_successful: - is_successful = True - - return is_successful - -########################################################################################################### -# Port Drayage 24: As the CMV approaches its destination, the CMV's actual trajectory will include a -# deceleration section. The average deceleration over the entire section shall be no -# less than 1 m/s^2, and the average deceleration over any 1-second portion of the section -# shall be no greater than 2.0 m/s^2. -########################################################################################################### -def check_deceleration_at_end_of_route(bag, engagement_times): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - # Set target speed (an offset above 0 mph, since deceleration rate will decrease significantly after this point) - target_speed_ms = 0.1 #0.89 # 0.89 m/s is 2 mph - - # Obtain the timestamp of the last speed increase before the system is disengaged; this is the start of deceleration - prev_speed = 0.0 - first = True - time_start_decel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement, end_time = time_end_engagement): - if first: - prev_speed = msg.speed - first = False - continue - - # Track the most recent occurrence of acceleration - delta_speed = msg.speed - prev_speed - if delta_speed >= 0.001: # Speed >= 0.001 kph (0.0006 mph) considered an acceleration - time_start_decel = t - speed_start_decel_ms = msg.speed * 0.277777 # Conversion from kph to m/s - #print("Time " + str(t.to_sec()) + " has speed " + str(speed_after_final_decel)) - - prev_speed = msg.speed - - speed_start_decel_mph = speed_start_decel_ms * 2.23694 # 2.23694 mph is 1 m/s - - # Obtain timestamp associated with the end of the deceleration section - # Note: This is the moment when the vehicle's speed reaches some offset of 0 mph - prev_speed = 0.0 - first = True - time_end_decel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_decel): - if first: - prev_speed = msg.speed - first = False - continue - - delta_speed = msg.speed - prev_speed - current_speed_ms = msg.speed * 0.27777 # Conversion of kph to m/s - if (current_speed_ms <= target_speed_ms): - time_end_decel = t - speed_end_decel_ms = current_speed_ms - speed_end_decel_mph = speed_end_decel_ms * 2.2369 # 2.2369 mph is 1 m/s - #print("Speed at end of acceleration section: " + str(speed_end_accel_mph) + " mph") - break - - prev_speed = msg.speed - - # Calculate the average deceleration across the full deceleration section - total_average_decel = (speed_start_decel_ms - speed_end_decel_ms) / (time_end_decel - time_start_decel).to_sec() - - # Obtain average decelation over all 1-second windows in deceleration section - one_second_duration = rospy.Duration(1.0) - all_one_second_windows_successful = True - max_one_second_decel = 0.0 - one_second_failures = 0 - first_one_second_failure = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_decel, end_time = (time_end_decel-one_second_duration)): - speed_initial_ms = msg.speed * 0.277777 # Conversion from kph to m/s - time_initial = t - - speed_final_ms = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): - speed_final_ms = msg.speed * 0.277777 # Conversion from kph to m/s - t_final = t - break - - one_second_decel = abs((speed_initial_ms - speed_final_ms) / (t_final-time_initial).to_sec()) - - if one_second_decel > max_one_second_decel: - max_one_second_decel = one_second_decel - if one_second_decel > 2.0: - if first_one_second_failure: - time_first_one_second_failure = t - first_one_second_failure = False - one_second_failures += 1 - #print("Failure: 1-second window had decel rate " + str(one_second_decel) + " m/s^2; end time was " + str((time_end_decel - t_final).to_sec()) + " seconds before complete stop") - all_one_second_windows_successful = False - #else: - # print("Success: 1-second window has decel rate at " + str(one_second_decel) + " m/s^2") - - # Print success/failure statements - total_deceleration_rate_successful = False - if total_average_decel >= 1.0: - print("PD-24-" + str(engagement_times[2]) + " (total decel at route end) succeeded; total average deceleration was " + str(total_average_decel) + " m/s^2; start speed: " + str(speed_start_decel_mph) + " mph") - total_deceleration_rate_successful = True - else: - print("PD-24-" + str(engagement_times[2]) + " (total decel at route end) failed; total average deceleration was " + str(total_average_decel) + " m/s^2; start speed: " + str(speed_start_decel_mph) + " mph") - - if all_one_second_windows_successful: - print("PD-24-" + str(engagement_times[2]) + " (1-second decel at route end) succeeded; no occurrences of 1-second average deceleration above 2.0 m/s^2; max: " + str(max_one_second_decel) + " m/s^2") - else: - print("PD-24-" + str(engagement_times[2]) + " (1-second decel at route end) failed; " + str(one_second_failures) + " failures starting " + str((time_end_decel - time_first_one_second_failure).to_sec()) + " sec before stop. Max: " + str(max_one_second_decel) + " m/s^2") - - - is_successful = False - if total_deceleration_rate_successful and all_one_second_windows_successful: - is_successful = True - return is_successful - -########################################################################################################### -# Port Drayage 25: After the CMV arrives to its destination, the UI shall successfully show a -# "Route Completed" dialog within 3.0 seconds. -########################################################################################################### -def check_route_completed_notification(bag, engagement_times): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - # Obtain the timestamp associated with the vehicle coming to a complete stop - time_complete_stop = rospy.Time() - fifteen_secon_duration = rospy.Duration(15.0) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = (time_start_engagement+fifteen_secon_duration)): - if msg.twist.linear.x <= 0.001: - time_complete_stop = t - break - - # Obtain the timestamp associated with the 'ROUTE_COMPLETED' notification occurring - time_route_completed_notification = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/guidance/route_event'], start_time = time_start_engagement): - # Event == 3 is 'ROUTE_COMPLETED' - if msg.event == 3: - time_route_completed_notification = t - break - - time_for_notification_to_occur = (time_route_completed_notification - time_complete_stop).to_sec() - - is_successful = False - if time_for_notification_to_occur < 3.0: - print("PD-25-" + str(engagement_times[2]) + " Succeeded; Route Completed notification occurred " + str(time_for_notification_to_occur) + " sec after complete stop.") - is_successful = True - else: - print("PD-25-" + str(engagement_times[2]) + " Failed; Route Completed notification occurred " + str(time_for_notification_to_occur) + " sec after complete stop. (Less than 3 sec required)") - is_successful = False - - return is_successful - -########################################################################################################### -# Port Drayage 18: The vehicle is able to send an arrival message to the infrastructure. - -# Port Drayage 9: The infrastructure is able to communicate the next destination to the vehicle. - -# Port Drayage 20: The vehicle is able to receive mobility operations messages from the infrastructure. - -# Port Drayage 10: The vehicle is able to receive the next destination from the infrastructure and -# display a message on Web UI to the user to proceed to that destination point. - -# Port Drayage 21: After the CMV has broadcasted its arrival message to V2XHub, the CMV shall receive a -# valid response from V2XHub including the next instructed destination in less than 1.5 seconds. - -# Port Drayage 22: After the CMV has received a message from V2XHub instructing it to proceed to its next destination, -# the CMV shall successfully generate an active route to that destination in less than 3 seconds. -########################################################################################################### -def check_cmv_arrival_message(bag, engagement_times, arrival_operation, next_destination_operation): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - max_seconds_before_next_destination_received = 1.5 # 1.5 seconds is max time between broadcasting arrival message and receiving the next destination - max_seconds_before_route_set = 3.0 # 3.0 seconds is max time between receiving a new destination and a route being set to that destination - - # Check whether the first 'carma/port_drayage' outgoing MobilityOperation message for this engagement section is the expected arrival message - time_arrival_message = rospy.Time() - pd_18_is_successful = False - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_operation'], start_time = time_start_engagement): - if msg.strategy == "carma/port_drayage": - if arrival_operation in msg.strategy_params: - pd_18_is_successful = True - time_arrival_message = t - break - - # Check metrics related to the response from V2XHub, which shall include the next destination - time_next_destination_received = rospy.Time() - pd_20_is_successful = False - pd_9_is_successful = False - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_operation'], start_time = time_arrival_message): - if msg.strategy == "carma/port_drayage": - if next_destination_operation in msg.strategy_params: - pd_20_is_successful = True - pd_9_is_successful = True - time_next_destination_received = t - break - - # Check whether destination message is received within the acceptable amount of time - pd_21_is_successful = False - time_between_arrival_and_next_destination_received = (time_next_destination_received - time_arrival_message).to_sec() - if (time_next_destination_received - time_arrival_message).to_sec() < max_seconds_before_next_destination_received: - pd_21_is_successful = True - - # Check that the Web UI received prompt to display a popup to the user for the new destination - pd_10_is_successful = False - time_between_new_destination_and_ui_popup = 0.0 - five_second_duration = rospy.Duration(5.0) - for topic, msg, t in bag.read_messages(topics=['/ui/ui_instructions'], start_time = time_next_destination_received, end_time = time_next_destination_received + five_second_duration): - if "A new Port Drayage route with operation type" in msg.msg: - pd_10_is_successful = True - time_between_new_destination_and_ui_popup = (t - time_next_destination_received).to_sec() - break - - # Check that new route is set within 3.0 seconds of the new destination being received from V2XHub - pd_22_is_successful = False - ten_second_duration = rospy.Duration(10.0) - time_route_is_set = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/guidance/route_event'], start_time = time_next_destination_received, end_time = time_next_destination_received + ten_second_duration): - - if msg.event == 2: # 2 is 'ROUTE_STARTED' - time_route_is_set = t - break - - time_between_new_destination_and_route_set = (time_route_is_set - time_next_destination_received).to_sec() - if time_between_new_destination_and_route_set < max_seconds_before_route_set: - pd_22_is_successful = True - - if pd_18_is_successful: - print("PD-18-" + str(engagement_times[2]) + " Succeeded; arrival message for " + str(arrival_operation) + " broadcasted.") - else: - print("PD-18-" + str(engagement_times[2]) + " Failed; arrival message for " + str(arrival_operation) + " not broadcasted.") - - if pd_9_is_successful: - print("PD-9-" + str(engagement_times[2]) + " Succeeded; CMV received a new destination from infrastructure.") - else: - print("PD-9-" + str(engagement_times[2]) + " Succeeded; CMV did not receive a new destination from infrastructure.") - - if pd_20_is_successful: - print("PD-20-" + str(engagement_times[2] + 1) + " Succeeded; received message for next destination: " + str(next_destination_operation) + " broadcasted.") - else: - print("PD-20-" + str(engagement_times[2] + 1) + " Failed; arrival message for " + str(next_destination_operation) + " not broadcasted.") - - if pd_10_is_successful: - print("PD-10-" + str(engagement_times[2] + 1) + " Succeeded; Web UI popup occurred " + str(time_between_new_destination_and_ui_popup) + " seconds after receiving new destination") - else: - print("PD-10-" + str(engagement_times[2] + 1) + " Failed; Web UI popup did not occur for new received destination.") - - if pd_21_is_successful: - print("PD-21-" + str(engagement_times[2] + 1) + " Succeeded; time between arrival broadcasted and next destination received: " + str(time_between_arrival_and_next_destination_received) + " seconds") - else: - print("PD-21-" + str(engagement_times[2] + 1) + " Failed; time between arrival broadcasted and next destination received: " + str(time_between_arrival_and_next_destination_received) + " seconds") - - if pd_22_is_successful: - print("PD-22-" + str(engagement_times[2] + 1) + " Succeeded; new route set " + str(time_between_new_destination_and_route_set) + " seconds after receiving new route.") - else: - print("PD-22-" + str(engagement_times[2] + 1) + " Failed; new route set " + str(time_between_new_destination_and_route_set) + " seconds after receiving new route.") - - return pd_18_is_successful, pd_9_is_successful, pd_20_is_successful, pd_10_is_successful, pd_21_is_successful, pd_22_is_successful - -########################################################################################################### -# Port Drayage XX: STOPPING LOCATION -########################################################################################################### -def check_stop_location(bag, engagement_times, stop_location_in_map, max_distance, metric_number, vehicle_length): - time_start_engagement = engagement_times[0] - time_end_engagement = engagement_times[1] - - map_x_stop_location = stop_location_in_map[0] - map_y_stop_location = stop_location_in_map[1] - #print(str(map_x_stop_location) + ", " + str(map_y_stop_location)) - - # Obtain the timestamp associated with the 'ROUTE_COMPLETED' notification occurring - time_route_completed_notification = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/guidance/route_event'], start_time = time_start_engagement): - # Event == 3 is 'ROUTE_COMPLETED' - if msg.event == 3: - time_route_completed_notification = t - break - - # Get the map location associated with the vehicle when it stopped - vehicle_stop_location_x = 0.0 # Vehicle x-cooridnate when it has stopped (in map frame) - vehicle_stop_location_y = 0.0 # Vehicle y-coordinate when it has stopped (in map frame) - for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_route_completed_notification): - vehicle_stop_location_x = msg.pose.position.x - vehicle_stop_location_y = msg.pose.position.y - #print(str(vehicle_stop_location_x) + ", " + str(vehicle_stop_location_y)) - break - - # Obtain distance between vehicle stop location and the desired stopping location - distance_from_desired_stopping_location = math.sqrt((vehicle_stop_location_x-map_x_stop_location)**2 + (vehicle_stop_location_y-map_y_stop_location)**2) - distance_from_desired_stopping_location -= vehicle_length - #print("Distance from desired stopping location: " + str(distance_from_desired_stopping_location) + " meters") - - is_successful = False - if distance_from_desired_stopping_location > 0: - if distance_from_desired_stopping_location < max_distance: - print("PD-" + str(metric_number) + "-" + str(engagement_times[2]) + " Succeeded; vehicle stopped " + str(distance_from_desired_stopping_location) + - " meters before stop point (Expected: Less than " + str(max_distance) + " meters before point)") - is_successful = True - else: - print("PD-" + str(metric_number) + "-" + str(engagement_times[2]) + " Failed; vehicle stopped " + str(distance_from_desired_stopping_location) + - " meters before stop point (Expected: Less than " + str(max_distance) + " meters before point)") - else: - print("PD-" + str(metric_number) + "-" + str(engagement_times[2]) + " Failed; vehicle stopped " + str(abs(distance_from_desired_stopping_location)) + - " meters after the stop point (Expected: Less than " + str(max_distance) + " meters before point)") - - return is_successful - -# Main Function; run all tests from here -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - source_folder = sys.argv[1] - - # Re-direct the output of print() to a specified .txt file: - orig_stdout = sys.stdout - current_time = datetime.datetime.now() - text_log_filename = "Results_" + str(current_time) + ".txt" - text_log_file_writer = open(text_log_filename, 'w') - sys.stdout = text_log_file_writer - - # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - csv_results_filename = "Results_" + str(current_time) + ".csv" - csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", - "pd_1_result_0", - "pd_4_result_0", "pd_4_result_3", "pd_4_result_9", - "pd_19_result_1", "pd_19_result_2", "pd_19_result_4", "pd_19_result_5", "pd_19_result_6", "pd_19_result_7", "pd_19_result_8", - "pd_23_result_0", "pd_23_result_1", "pd_23_result_2", "pd_23_result_3", "pd_23_result_4", "pd_23_result_5", "pd_23_result_6", "pd_23_result_7", "pd_23_result_8", "pd_23_result_9", - "pd_24_result_0", "pd_24_result_1", "pd_24_result_2", "pd_24_result_3", "pd_24_result_4", "pd_24_result_5", "pd_24_result_6", "pd_24_result_7", "pd_24_result_8", - "pd_25_result_0", "pd_25_result_1", "pd_25_result_2", "pd_25_result_3", "pd_25_result_4", "pd_25_result_5", "pd_25_result_6", "pd_25_result_7", "pd_25_result_8", - "pd_18_result_0", "pd_18_result_1", "pd_18_result_2", "pd_18_result_3", "pd_18_result_4", "pd_18_result_5", "pd_18_result_6", "pd_18_result_7", "pd_18_result_8", - "pd_9_result_0", "pd_9_result_2", "pd_9_result_3", "pd_9_result_8", - "pd_20_result_0", "pd_20_result_1", "pd_20_result_2", "pd_20_result_3", "pd_20_result_4", "pd_20_result_5", "pd_20_result_6", "pd_20_result_7", "pd_20_result_8", "pd_20_result_9", - "pd_10_result_0", "pd_10_result_1", "pd_10_result_2", "pd_10_result_3", "pd_10_result_4", "pd_10_result_5", "pd_10_result_6", "pd_10_result_7", "pd_10_result_8", "pd_10_result_9", - "pd_21_result_1", "pd_21_result_2", "pd_21_result_3", "pd_21_result_4", "pd_21_result_5", "pd_21_result_6", "pd_21_result_7", "pd_21_result_8", "pd_21_result_9", - "pd_22_result_0", "pd_22_result_1", "pd_22_result_2", "pd_22_result_3", "pd_22_result_4", "pd_22_result_5", "pd_22_result_6", "pd_22_result_7", "pd_22_result_8", "pd_22_result_9", - "pd_8_result_1", "pd_8_result_4", "pd_8_result_5", "pd_8_result_6", "pd_8_result_7", "pd_8_result_8", - "pd_2_result_0", "pd_2_result_3", - "pd_11_result_1", "pd_11_result_4", "pd_11_result_5", - "pd_5_result_6", - "pd_7_result_7", - "pd_3_result_3", "pd_3_result_9", - "pd_6_result_6", - "pd_12_result_1", "pd_12_result_5", - "pd_13_result_1", "pd_13_result_5", - "pd_14_result_4", - "pd_15_result_1", "pd_15_result_5", - "pd_16_result_4", - "pd_17_result_4"]) - - silver_truck_bag_files = ["1P1-R3-TS-12-9-21.bag"] - #["1P1-R1-TS-12-9-21.bag", - # "1P1-R2-TS-12-9-21.bag", - # "1P1-R3-TS-12-9-21.bag"] - - # Concatenate all Port Drayage bag file lists into one list - PD_bag_files = silver_truck_bag_files - - # Loop to conduct data anlaysis on each bag file: - for bag_file in PD_bag_files: - print("*****************************************************************") - print("Processing new bag: " + str(bag_file)) - if bag_file in silver_truck_bag_files: - print("Silver Truck Port Drayage Test Case") - else: - print("Unknown bag file being processed.") - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(PD_bag_files.index(bag_file) + 1) + " of " + str(len(PD_bag_files)) + ")") - sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - bag = rosbag.Bag(bag_file_path) - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - # Get the rosbag start and end times associated with each "System Engaged" portion of the Use Case. - # Note: "System Engaged" sections for a Port Drayage use case are listed below: - # 0. Test Start Location ------> Staging Area Entrance - # 1. Staging Area Entrance ----> Staging Area Pickup - # 2. Staging Area Pickup ------> Staging Area Exit - # 3. Staging Area Exit --------> Port Entrance - # 4. Port Entrance ------------> Port Dropoff - # 5. Port Dropoff -------------> Port Pickup - # 6. Port Pickup --------------> Port Checkpoint - # 7. Port Checkpoint ----------> Port Holding Area - # 8. Port Holding Area --------> Port Exit - # 9. Port Exit ----------------> Staging Area Entrance - num_engagement_sections = 10 # 10 expected engagement sections for the Port Drayage use case - start_times_list, end_times_list, found_engagement_times = get_test_case_engagement_times(bag, num_engagement_sections) - segment_0 = [start_times_list[0], end_times_list[0], 0] # Test Start Location ------> Staging Area Entrance - segment_1 = [start_times_list[1], end_times_list[1], 1] # Staging Area Entrance ----> Staging Area Pickup - segment_2 = [start_times_list[2], end_times_list[2], 2] # Staging Area Pickup ------> Staging Area Exit - segment_3 = [start_times_list[3], end_times_list[3], 3] # Staging Area Exit --------> Port Entrance - segment_4 = [start_times_list[4], end_times_list[4], 4] # Port Entrance ------------> Port Dropoff - segment_5 = [start_times_list[5], end_times_list[5], 5] # Port Dropoff -------------> Port Pickup - segment_6 = [start_times_list[6], end_times_list[6], 6] # Port Pickup --------------> Port Checkpoint - segment_7 = [start_times_list[7], end_times_list[7], 7] # Port Checkpoint ----------> Port Holding Area - segment_8 = [start_times_list[8], end_times_list[8], 8] # Port Holding Area --------> Port Exit - segment_9 = [start_times_list[9], end_times_list[9], 9] # Port Exit ----------------> Staging Area Entrance - - print_lanelet_entrance_times(bag, segment_0[0]) - # Finished: - # 1 ( 0, , , , , , , , , ) - # 4 ( 0, , , 3, , , , , ,I9) - # 19 ( 1, 2, , 4, 5, 6, 7, 8, ) - # 23 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, 9) - # 24 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, ) - # 25 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, ) - # 18 ( 0, 1, 2, 3, 4, 5, 6, 7, 8, ) # CMV sends arrival message - # 9 ( 0, , 2, 3, , , , , 8, ) # V2XHub sends next destination after arrival - # 20 (I0, 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV can receive MobilityOperation messages from V2XHub - # 10 (I0, 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV receives the next destination from V2XHub - # 21 ( , 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV receives next destination < 1.5 sec after sending arrival - # 22 (I0, 1, 2, 3, 4, 5, 6, 7, 8, 9) # CMV generates route to next destination in < 3.0 sec - # 8 ( , 1, , , 4, 5, 6, 7, 8, ) - # 2 ( 0, , , 3, , , , , , ) - # 11 ( , 1, , , 4, 5, , , , ) - # 5 ( , , , , , , 6, , , ) - # 7 ( , , , , , , , 7, , ) - # 3 ( , , , 3, , , , , , 9) - # 6 ( , , , , , ,V6, , , ) # Requires V2XHub Logs (Test Operator can select either Holding Area or Port Exit) - # 12 ( ,V1, , , ,V5, , , , ) # Requires V2XHub Logs (Test Operator Communicates to V2XHub that container is loaded) - # 13 ( , 1, , , , 5, , , , ) - # 14 ( , , , , 4, , , , , ) - # 15 ( ,V1, , , ,V5, , , , ) # Requires V2XHub Logs (V2XHub sends loading command to Test Operator) - # 16 ( , , , ,V4, , , , , ) # Requires V2XHub Logs (V2XHub sends unloading command to Test Operator) - # 17 ( , , , ,V4, , , , , ) # Requires V2XHub Logs (Test Operator Communicates to V2XHub that container is unloaded) - - # Metric FPD-1 (Vehicle reaches +/- 2 mph of speed limit) - speed_limit_outside_port_and_staging_area_ms = 8.94 # 8.94 m/s is 20 mph - pd_1_result_0 = check_vehicle_reaches_steady_state(bag, segment_0, speed_limit_outside_port_and_staging_area_ms) - - # Metric FPD-4 (Speed limit outside Staging Area & Port is 20-25 mph) - min_speed_limit_FPD4 = 8.94 # 8.94 m/s is 20 mph - max_speed_limit_FPD4 = 11.18 # 11.18 m/s is 25 mph - segment_0_lanelet_ids = [65787, 24943, 76157] # Ordered lanelet IDs encountered in this segment that are outside of Staging Area and Port - segment_3_lanelet_ids = [75649, 25151, 66085, 16359, 18269, 82313] # Ordered lanelet IDs encountered in this segment that are outside of Staging Area and Port - pd_4_result_0 = check_speed_limit_outside_port_and_staging_area(bag, segment_0, segment_0_lanelet_ids, min_speed_limit_FPD4, max_speed_limit_FPD4) - pd_4_result_3 = check_speed_limit_outside_port_and_staging_area(bag, segment_3, segment_3_lanelet_ids, min_speed_limit_FPD4, max_speed_limit_FPD4) - - # FPD-19 (Speed limit inside Staging Area or Port is 10 mph) - speed_limit_inside_port_and_staging_area_ms = 4.47 # 4.47 m/s is 10 mph - pd_19_result_1 = check_speed_limit_inside_port_and_staging_area(bag, segment_1, speed_limit_inside_port_and_staging_area_ms) - pd_19_result_2 = check_speed_limit_inside_port_and_staging_area(bag, segment_2, speed_limit_inside_port_and_staging_area_ms) - pd_19_result_4 = check_speed_limit_inside_port_and_staging_area(bag, segment_4, speed_limit_inside_port_and_staging_area_ms) - pd_19_result_5 = check_speed_limit_inside_port_and_staging_area(bag, segment_5, speed_limit_inside_port_and_staging_area_ms) - pd_19_result_6 = check_speed_limit_inside_port_and_staging_area(bag, segment_6, speed_limit_inside_port_and_staging_area_ms) - pd_19_result_7 = check_speed_limit_inside_port_and_staging_area(bag, segment_7, speed_limit_inside_port_and_staging_area_ms) - pd_19_result_8 = check_speed_limit_inside_port_and_staging_area(bag, segment_8, speed_limit_inside_port_and_staging_area_ms) - - # FPD-23 (Performance requirements for acceleration at start of route) - pd_23_result_0 = check_acceleration_after_engagement(bag, segment_0, speed_limit_outside_port_and_staging_area_ms) - pd_23_result_1 = check_acceleration_after_engagement(bag, segment_1, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_2 = check_acceleration_after_engagement(bag, segment_2, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_3 = check_acceleration_after_engagement(bag, segment_3, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_4 = check_acceleration_after_engagement(bag, segment_4, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_5 = check_acceleration_after_engagement(bag, segment_5, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_6 = check_acceleration_after_engagement(bag, segment_6, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_7 = check_acceleration_after_engagement(bag, segment_7, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_8 = check_acceleration_after_engagement(bag, segment_8, speed_limit_inside_port_and_staging_area_ms) - pd_23_result_9 = check_acceleration_after_engagement(bag, segment_9, speed_limit_inside_port_and_staging_area_ms) - - # FPD-24 (Performance requirements for deceleration at end of route) - pd_24_result_0 = check_deceleration_at_end_of_route(bag, segment_0) - pd_24_result_1 = check_deceleration_at_end_of_route(bag, segment_1) - pd_24_result_2 = check_deceleration_at_end_of_route(bag, segment_2) - pd_24_result_3 = check_deceleration_at_end_of_route(bag, segment_3) - pd_24_result_4 = check_deceleration_at_end_of_route(bag, segment_4) - pd_24_result_5 = check_deceleration_at_end_of_route(bag, segment_5) - pd_24_result_6 = check_deceleration_at_end_of_route(bag, segment_6) - pd_24_result_7 = check_deceleration_at_end_of_route(bag, segment_7) - pd_24_result_8 = check_deceleration_at_end_of_route(bag, segment_8) - - # FPD-25 (Route Completed notification occurs within 3 seconds of reaching destination) - pd_25_result_0 = check_route_completed_notification(bag, segment_0) - pd_25_result_1 = check_route_completed_notification(bag, segment_1) - pd_25_result_2 = check_route_completed_notification(bag, segment_2) - pd_25_result_3 = check_route_completed_notification(bag, segment_3) - pd_25_result_4 = check_route_completed_notification(bag, segment_4) - pd_25_result_5 = check_route_completed_notification(bag, segment_5) - pd_25_result_6 = check_route_completed_notification(bag, segment_6) - pd_25_result_7 = check_route_completed_notification(bag, segment_7) - pd_25_result_8 = check_route_completed_notification(bag, segment_8) - - # FPD-18 (CMV broadcasts arrival message to infrastructure) - # Do one function that takes the 'OPERATION' as a passed argument - pd_18_result_0, pd_9_result_0, pd_20_result_1, pd_10_result_1, pd_21_result_1, pd_22_result_1 = check_cmv_arrival_message(bag, segment_0, "\"operation\": \"ENTER_STAGING_AREA\"", "\"operation\": \"PICKUP\"") - pd_18_result_1, pd_9_result_1, pd_20_result_2, pd_10_result_2, pd_21_result_2, pd_22_result_2 = check_cmv_arrival_message(bag, segment_1, "\"operation\": \"PICKUP\"", "\"operation\": \"EXIT_STAGING_AREA\"") - pd_18_result_2, pd_9_result_2, pd_20_result_3, pd_10_result_3, pd_21_result_3, pd_22_result_3 = check_cmv_arrival_message(bag, segment_2, "\"operation\": \"EXIT_STAGING_AREA\"", "\"operation\": \"ENTER_PORT\"") - pd_18_result_3, pd_9_result_3, pd_20_result_4, pd_10_result_4, pd_21_result_4, pd_22_result_4 = check_cmv_arrival_message(bag, segment_3, "\"operation\": \"ENTER_PORT\"", "\"operation\": \"DROPOFF\"") - pd_18_result_4, pd_9_result_4, pd_20_result_5, pd_10_result_5, pd_21_result_5, pd_22_result_5 = check_cmv_arrival_message(bag, segment_4, "\"operation\": \"DROPOFF\"", "\"operation\": \"PICKUP\"") - pd_18_result_5, pd_9_result_5, pd_20_result_6, pd_10_result_6, pd_21_result_6, pd_22_result_6 = check_cmv_arrival_message(bag, segment_5, "\"operation\": \"PICKUP\"", "\"operation\": \"PORT_CHECKPOINT\"") - pd_18_result_6, pd_9_result_6, pd_20_result_7, pd_10_result_7, pd_21_result_7, pd_22_result_7 = check_cmv_arrival_message(bag, segment_6, "\"operation\": \"PORT_CHECKPOINT\"", "\"operation\": \"HOLDING_AREA\"") - pd_18_result_7, pd_9_result_7, pd_20_result_8, pd_10_result_8, pd_21_result_8, pd_22_result_8 = check_cmv_arrival_message(bag, segment_7, "\"operation\": \"HOLDING_AREA\"", "\"operation\": \"EXIT_PORT\"") - pd_18_result_8, pd_9_result_8, pd_20_result_9, pd_10_result_9, pd_21_result_9, pd_22_result_9 = check_cmv_arrival_message(bag, segment_8, "\"operation\": \"EXIT_PORT\"", "\"operation\": \"ENTER_STAGING_AREA\"") - - # Check FPD-13 Segments 1 and 5 (Infrastructure communicates to CMV that container is loaded) - if (pd_10_result_2): - pd_13_result_1 = True - print("PD-13-1 Succeeded; CMV was notified that container was loaded.") - else: - pd_13_result_1 = False - print("PD-13-1 Failed; CMV was not notified that container was loaded.") - - if (pd_10_result_6): - pd_13_result_5 = True - print("PD-13-5 Succeeded; CMV was notified that container was loaded.") - else: - pd_13_result_5 = False - print("PD-13-5 Failed; CMV was not notified that container was loaded.") - - # Check FPD-14 Segment 4 (Infrastructure communicates to CMV that container is unloaded) - if (pd_10_result_5): - pd_14_result_4 = True - print("PD-14-4 Succeeded; CMV was notified that container was loaded.") - else: - pd_14_result_4 = False - print("PD-14-4 Failed; CMV was not notified that container was loaded.") - - # PD-8 is always true (System engages on route after user chooses to engage from the Web UI) - # PD-9 is always true (System can exit Staging Area or Port after receiving command from V2XHub) - # Note: These metrics were evaluated in the vehicle, not by the data analysis script. - pd_8_result_1 = True - pd_3_result_3 = True - pd_8_result_4 = True - pd_8_result_5 = True - pd_8_result_6 = True - pd_8_result_7 = True - pd_8_result_8 = True - pd_3_result_9 = True - - # FPD-2, FPD-11, and FPD-5 (stopping location metrics) - # Note: These map stopping locations were obtained by using the Lat/Long stopping locations from the - # use case and transforming them to the map frame via unit tests contained in the carma-platform Route package. - stop_location_0 = [-401.128, 515.763] # [map_x, map_y] - stop_location_1 = [-414.198, 709.169] # [map_x, map_y] - stop_location_2 = [-408.123, 514.201] # [map_x, map_y] # No metric exists to evaluate this stop - stop_location_3 = [-75.2801, -647.096] # [map_x, map_y] - stop_location_4 = [-52.0422, -731.793] # [map_x, map_y] - stop_location_5 = [-28.6465, -758.279] # [map_x, map_y] - stop_location_6 = [2.51176, -723.625] # [map_x, map_y] - stop_location_7 = [-3.0006, -647.181] # [map_x, map_y] - stop_location_8 = [-112.047, -506.881] # [map_x, map_y] # No metric exists to evaluate this stop - - freightliner_vehicle_length = 6.0 - pd_2_result_0 = check_stop_location(bag, segment_0, stop_location_0, max_distance = 3, metric_number = 2, vehicle_length = freightliner_vehicle_length) - pd_11_result_1 = check_stop_location(bag, segment_1, stop_location_1, max_distance = 10, metric_number = 11, vehicle_length = freightliner_vehicle_length) - pd_2_result_3 = check_stop_location(bag, segment_3, stop_location_3, max_distance = 3, metric_number = 2, vehicle_length = freightliner_vehicle_length) - pd_11_result_4 = check_stop_location(bag, segment_4, stop_location_4, max_distance = 10, metric_number = 11, vehicle_length = freightliner_vehicle_length) - pd_11_result_5 = check_stop_location(bag, segment_5, stop_location_5, max_distance = 10, metric_number = 11, vehicle_length = freightliner_vehicle_length) - pd_5_result_6 = check_stop_location(bag, segment_6, stop_location_6, max_distance = 3, metric_number = 5, vehicle_length = freightliner_vehicle_length) - pd_7_result_7 = check_stop_location(bag, segment_7, stop_location_7, max_distance = 3, metric_number = 7, vehicle_length = freightliner_vehicle_length) - - # Invalid Metrics (Either not-applicable or the metrics are not for carma-platform): - pd_4_result_9 = None - pd_20_result_0 = None - pd_10_result_0 = None - pd_22_result_0 = None - pd_6_result_6 = None - pd_12_result_1 = None - pd_12_result_5 = None - pd_15_result_1 = None - pd_15_result_5 = None - pd_16_result_4 = None - pd_17_result_4 = None - - # Get vehicle type that this bag file is from - vehicle_name = "Unknown" - if bag_file in silver_truck_bag_files: - vehicle_name = "Silver Truck" - else: - vehicle_name = "N/A" - - # Get test type that this bag file is for - vehicle_role = "Port Drayage" - - # Write simple pass/fail results to .csv file for appropriate row: - csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, - pd_1_result_0, - pd_4_result_0, pd_4_result_3, pd_4_result_9, - pd_19_result_1, pd_19_result_2, pd_19_result_4, pd_19_result_5, pd_19_result_6, pd_19_result_7, pd_19_result_8, - pd_23_result_0, pd_23_result_1, pd_23_result_2, pd_23_result_3, pd_23_result_4, pd_23_result_5, pd_23_result_6, pd_23_result_7, pd_23_result_8, pd_23_result_9, - pd_24_result_0, pd_24_result_1, pd_24_result_2, pd_24_result_3, pd_24_result_4, pd_24_result_5, pd_24_result_6, pd_24_result_7, pd_24_result_8, - pd_25_result_0, pd_25_result_1, pd_25_result_2, pd_25_result_3, pd_25_result_4, pd_25_result_5, pd_25_result_6, pd_25_result_7, pd_25_result_8, - pd_18_result_0, pd_18_result_1, pd_18_result_2, pd_18_result_3, pd_18_result_4, pd_18_result_5, pd_18_result_6, pd_18_result_7, pd_18_result_8, - pd_9_result_0, pd_9_result_2, pd_9_result_3, pd_9_result_8, - pd_20_result_0, pd_20_result_1, pd_20_result_2, pd_20_result_3, pd_20_result_4, pd_20_result_5, pd_20_result_6, pd_20_result_7, pd_20_result_8, pd_20_result_9, - pd_10_result_0, pd_10_result_1, pd_10_result_2, pd_10_result_3, pd_10_result_4, pd_10_result_5, pd_10_result_6, pd_10_result_7, pd_10_result_8, pd_10_result_9, - pd_21_result_1, pd_21_result_2, pd_21_result_3, pd_21_result_4, pd_21_result_5, pd_21_result_6, pd_21_result_7, pd_21_result_8, pd_21_result_9, - pd_22_result_0, pd_22_result_1, pd_22_result_2, pd_22_result_3, pd_22_result_4, pd_22_result_5, pd_22_result_6, pd_22_result_7, pd_22_result_8, pd_22_result_9, - pd_8_result_1, pd_8_result_4, pd_8_result_5, pd_8_result_6, pd_8_result_7, pd_8_result_8, - pd_2_result_0, pd_2_result_3, - pd_11_result_1, pd_11_result_4, pd_11_result_5, - pd_5_result_6, - pd_7_result_7, - pd_3_result_3, pd_3_result_9, - pd_6_result_6, - pd_12_result_1, pd_12_result_5, - pd_13_result_1, pd_13_result_5, - pd_14_result_4, - pd_15_result_1, pd_15_result_5, - pd_16_result_4, - pd_17_result_4]) - - sys.stdout = orig_stdout - text_log_file_writer.close() - return - -if __name__ == "__main__": - main() diff --git a/engineering_tools/data_processing/analyze_tsmo_uc2_rosbags.py b/engineering_tools/data_processing/analyze_tsmo_uc2_rosbags.py deleted file mode 100644 index c9006dc95c..0000000000 --- a/engineering_tools/data_processing/analyze_tsmo_uc2_rosbags.py +++ /dev/null @@ -1,443 +0,0 @@ -#!/usr/bin/python3 - -# 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. - -from cmath import phase -from inspect import TPFLAGS_IS_ABSTRACT -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime -import math - -########################################################################################################### -# TSMO UC1-M1.1 : Vehicles should receive SPaT messages from RSU -# once the vehicles enter the communication area until the vehicles depart the intersection box. -# Each vehicle processes this information and use it as inputs of the trajectory planning-related features -# (i.e., TS, the current CARMA Platform path following feature). -# The SPaT message information shall include timestamp, cycle length, phase sequence, phase durations, -# current phase ID, and the remaining of the current phase durantion. -# -# Expected: True -# TODO implement -########################################################################################################### -''' -Required topics -- SPaT Data : /message/incoming_spat cav_msgs/SPAT -Other Required information -- Movement Ids from the Spat that are needed for the vehicle and scenario being tested (may need to be a hard coded mapping) -- Intersection Id for the spat. Since we have two intersections but only one is used in the test the id of interest should be hard coded into the analysis - With the expected id we can verify that the required correct spat message is received. -Approach: -- Spat will provide the current light phase state by matching the movement id for the current scenario into the plan -- Spat fields can be checked for availability to ensure the listed fields in the metric are available -- Intersection id of the spat message can be checked to ensure it is the correct message being analyzed -''' - - - -### -# Mike suggested. MAP messages received and processed and they contain intersection ids matching the expected values -# This when combined with above metric confirms all required v2i information is available -# TODO implement if extra time -### -''' -Required topics -- Map Data : /message/incoming_map cav_msgs/Map -Other Required Data: -- Map messages have intersection Ids. Since we have two intersections but only one is used in the test the id of interest should be hard coded into the analysis - With the expected id we can verify that the required map message is received. -''' - - -########################################################################################################### -# TSMO UC2-M1.2 : The communication from infrastructure (RSU) to vehicle (OBU) should be within a certain frequency rates -# -# Expected: 10 Hz ± 1 Hz for SPaT, 1 Hz ± 0.5 Hz for MA -# TODO implement -########################################################################################################### -''' -Required topics -- SPaT Data : /message/incoming_spat cav_msgs/SPAT -- Map Data : /message/incoming_map cav_msgs/Map -- Engagement status : /guidance/state cav_msgs/GuidanceState - -Approach: -- After the first 3 Spat messages have been received consider communication stable enough for analysis (this may be before or after engagement) -- Count the number of both spat and map starting from engagement to disengagement. - Divide this number by the total time period of engagement to get total average frequency for each message -- Additionally, create a 1s sliding window for spat and a 5s sliding window for map and verify that the average frequency within these windows never falls outside the specified ranges - -Plotting: -- In addition to the pass/fail criterion plots should be generated of the frequency in the sliding windows at each timestep -''' - -########################################################################################################### -# TSMO UC2-M2.1 : Each vehicle should travel sequentially through the lanelets defined in its path. -# -# Expected: True -# TODO implement. Note the expected set of lanelets changes based on the approach being used by the vehicle -########################################################################################################### - -''' -Required topics -- Current lanelet data : /guidance/route_state cav_msgs/RouteState -- Engagement status : /guidance/state cav_msgs/GuidanceState - -Approach: -- Hard code the expected lanelet ids for each approach in each test scenario -- For a given run after the system is engaged, verify that the route_state reports the current lanelets in sequence following the same order as specified for the scenario and approach -''' - -########################################################################################################### -# -# NOTE: This metric may get tweaked. Suggest tackling last - -# TSMO UC2-M2.2 : Each vehicle shall process the SPaT message -# and adjust its speed such that it can enter the intersection box at a green phase with minimum or no stopping -# Specifically the vehicle should on average stop for less time then would be required by the baseline case of EET (Earliest entry time) -# -# Expected: Vehicle's actual stopping time is less then predicted from EET -# -# TODO implement -########################################################################################################### - -''' -Required topics -- Not a topic, but will need knowledge of current test case being evaluated to determine expected behavior. -- Current lanelet and downtrack: /guidance/route_state cav_msgs/RouteState -- Engagement status : /guidance/state cav_msgs/GuidanceState -- Current_speed : /hardware_interface/vehicle/twist geometry_msgs/TwistStamped (http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TwistStamped.html) -- SPaT Data : /message/incoming_spat cav_msgs/SPAT - -Other Required Data: -- Log file from lci_strategic_plugin with DEBUG logs enabled. This is needed to get the EET value -- Intersection and movement ids for identifying correct spat - -Approach: -- At the start of vehicle engagement look in the lci_strategic_plugin log file for the first occurrence of "earliest_entry_time" This will be the EET value. (from ROS_DEBUG_STREAM("earliest_entry_time: " << std::to_string(earliest_entry_time.toSec()) << ", with : " << earliest_entry_time - current_state.stamp << " left at: " << std::to_string(current_state.stamp.toSec()));) -- Using the EET value determine if the light was red, yellow, or green from SPAT -- If the light was red at EET then find the next time when the light turns green --- Save the delta between next green start and EET (will refer to as BaseCaseStopTime) --- Based on the data determine how much time the vehicle was at a stop at the stop bar while engaged (will refer to as VehicleStopTime) --- If VehicleStopTime < BaseCaseStopTime ----- Record a Successes (algorithm performs better than base case) -- If the light was green at EET --- If the vehicle stops in this case, ----- Record failure --- else ----- Record success -- If the light was yellow at EET --- If the vehicle stops ----- Record success --- If the vehicle does not stop ----- Record failure - - -''' - -########################################################################################################### -# TSMO UC2-M2.3 : Vehicles shall not enter the intersection box at yellow or red phases. -# -# Expected: True -# TODO implement -########################################################################################################### - -''' -Required topics -- Current lanelet data : /guidance/route_state cav_msgs/RouteState -- Engagement status : /guidance/state cav_msgs/GuidanceState -- SPaT Data : /message/incoming_spat cav_msgs/SPAT -Other Required Data: -- The intersection and movement ids expected for the vehicle and scenario being tested - -Approach: -- Use the intersection id and movement id to identify the current phase of the light which is required -- Use the route_state to determine the current lanelet of the vehicle -- While the vehicle is engaged, its reported lanelet should never be one inside the intersection while the phase state is not green -''' - -########################################################################################################### -# TSMO UC2-M2.4 : Each vehicle should switch away from the TS mode after entering the intersection box -# Note that the intersection box is the box per the HD map, which may not be exactly as physically seen on the road. -# Expected: True -# TODO implement -########################################################################################################### - -''' -Required topics -- Current lanelet data : /guidance/route_state cav_msgs/RouteState -- Engagement status : /guidance/state cav_msgs/GuidanceState -- Determine lci plugin is used in planning : /guidance/plan_trajectory cav_msgs/TrajectoryPlan - -Approach: -- When the vehicle is engaged, gets its current lanelet from the route state -- Verify that when the lanelet is one inside the intersection the current planning trajectory is not coming from the lci tactical plugin -- To check the controlling tactical plugin look at the planner_plugin field of the first point on the current plan_trajectory -''' - -########################################################################################################### -# TSMO UC2-M2.3 : The average deceleration\acceleration over any 1-second portion of the test time horizon -# shall be no greater than 3.0 m/s^2. -# -# Expected: acceleration <= 3.0 m/s^2 deceleration >= -3.0 m/s^2 -# TODO implement -########################################################################################################### - -''' -Required topics -- Engagement status : /guidance/state cav_msgs/GuidanceState -- Current_speed : /hardware_interface/vehicle/twist (use to compute accel) geometry_msgs/TwistStamped -- Vehicle accel : /hardware_interface/velocity_accel_cov (use as alternative source of accel) automotive_platform_msgs/VelocityAccelCov (https://github.com/astuff/automotive_autonomy_msgs/blob/master/automotive_platform_msgs/msg/VelocityAccelCov.msg) - -Approach: -- While the vehicle is engaged, create a 1s sliding window and take the start and end speed for each timestep of the window and compute the average acceleration -- Additionally, verify that the abs(/hardware_interface/velocity_accel_cov reported value) never exceeds 3.0 for more than 1 timestep at any point during engagement -''' - -### -# Mike suggested. Vehicle does not violate speed limit my more than 1 mph -# Good to confirm as this showed up as an issue multiple times during integration testing -# TODO implement if extra time -### -''' -Required topics -- Engagement status : /guidance/state cav_msgs/GuidanceState -- Current_speed : /hardware_interface/vehicle/twist geometry_msgs/TwistStamped -Other Required Data: -- Speed limit extracted from map - -Approach: -- In TSMO UC2 all the speed limits will be the same so a simple conditional while the vehicle is engaged on twist.linear.x will suffice to check if the limit is exceeded -''' - - -########################################################################################################### -# Additional Requested Plots: Speed over time with matching signal state shown -########################################################################################################### - -''' -Required topics -- Engagement status : /guidance/state -- Current_speed : /hardware_interface/vehicle/twist -- SPaT Data : /message/incoming_spat - -Other Required Data: -- Intersection id and movement id for the spat message - -Approach: -- While the vehicle is engaged plot the current speed twist.linear.x -- Also plot the signal phase as red, green, yellow bars on the same graph - -NOTE: This plot should be generated in two forms 1 time for the individual vehicles and once as a single plot combing the data from two vehicles -''' - -########################################################################################################### -# Additional Requested Plots: Downtrack position over time with matching signal state shown -########################################################################################################### - -''' -Required topics -- Engagement status : /guidance/state -- Current downtrack data : /guidance/route_state -- SPaT Data : /message/incoming_spat -Other Required Data: -- Intersection id and movement id for the spat message - -Approach: -- While the vehicle is engaged plot the current downtrack distance reported by route_state -- Identify where the stop bar is in terms of downtrack distance (this might change per run) -- At the position where the stop bar is (y-axis) draw a set of red/green/yellow rectangles showing the current signal phase - -NOTE: This plot should be generated in two forms 1 time for the individual vehicles and once as a single plot combing the data from two vehicles -''' - -########################################################################################################### -# Additional Requested Plots: Acceleration over time with matching signal state shown -########################################################################################################### - -''' -Required topics -- Engagement status : /guidance/state -- Current_speed : /hardware_interface/vehicle/twist (use to compute accel) -- Vehicle accel : /hardware_interface/velocity_accel_cov (use as alternative source of accel) -- SPaT Data : /message/incoming_spat -Other Required Data: -- Intersection id and movement id for the spat message - -Approach: -- While the vehicle is engaged plot the current acceleration -- Also plot the signal phase as red, green, yellow bars on the same graph - -NOTE: This plot should be generated in two forms 1 time for the individual vehicles and once as a single plot combing the data from two vehicles -''' - - -# Main Function; run all tests from here -# TODO: The contents of this main function provide some basic structure for loading data, but need not be followed if not applicable -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - source_folder = sys.argv[1] - - # Re-direct the output of print() to a specified .txt file: - orig_stdout = sys.stdout - current_time = datetime.datetime.now() - text_log_filename = "Results_" + str(current_time) + ".txt" - text_log_file_writer = open(text_log_filename, 'w') - sys.stdout = text_log_file_writer - - # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - csv_results_filename = "Results_" + str(current_time) + ".csv" - csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - - # TODO update this list - csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", - "WZ-1 Result", "WZ-2 Result", "WZ-3 Result", "WZ-4 Result", "WZ-5 Result", "WZ-6 Result", - "WZ-7 Result", "WZ-8 Result", "WZ-9 Result", "WZ-10 Result","WZ-11 Result", "WZ-12 Result", - "WZ-13 Result", "WZ-14 Result", "WZ-15 Result", "WZ-16 Result", "WZ-17 Result", "WZ-18 Result", - "WZ-19 Result", "WZ-20 Result", "WZ-21 Result", "WZ-22 Result", "WZ-23 Result", "WZ-24 Result", "WZ-25 Result"]) - - # TODO identify cases and assign bags to each case - # Create list of Red Light Workzone Black Pacifica bag files to be processed - black_pacifica_red_bag_files = [] - - # Create list of Red Light Workzone Ford Fusion bag files to be processed - ford_fusion_red_bag_files = ["_2021-09-22-16-00-28-red.bag", - "_2021-09-22-19-03-26-red.bag"] - - # Create list of Red Light Workzone Blue Lexus bag files to be processed - blue_lexus_red_bag_files = [] - - # Create list of Green Light Workzone Black Pacifica bag files to be processed - black_pacifica_green_bag_files = [] - - # Create list of Green Light Workzone Ford Fusion bag files to be processed - ford_fusion_green_bag_files = [] - - # Create list of Green Light Workzone Blue Lexus bag files to be processed - blue_lexus_green_bag_files = [] - - # Concatenate all Basic Travel bag files into one list - red_light_bag_files = black_pacifica_red_bag_files + ford_fusion_red_bag_files + blue_lexus_red_bag_files - green_light_bag_files = black_pacifica_green_bag_files + ford_fusion_green_bag_files + blue_lexus_green_bag_files - WZ_bag_files = red_light_bag_files + green_light_bag_files - - # Loop to conduct data anlaysis on each bag file: - for bag_file in WZ_bag_files: - print("*****************************************************************") - print("Processing new bag: " + str(bag_file)) - # TODO update bags groups - if bag_file in black_pacifica_red_bag_files: - print("Black Pacifica Red Light Workzone Test Case") # TODO update conditionals here - elif bag_file in ford_fusion_red_bag_files: - print("Ford Fusion Red Light Workzone Test Case") - elif bag_file in blue_lexus_red_bag_files: - print("Blue Lexus Red Light Workzone Test Case") - if bag_file in black_pacifica_green_bag_files: - print("Black Pacifica Green Light Workzone Test Case") - elif bag_file in ford_fusion_green_bag_files: - print("Ford Fusion Green Light Workzone Test Case") - elif bag_file in blue_lexus_green_bag_files: - print("Blue Lexus Green Light Workzone Test Case") - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(WZ_bag_files.index(bag_file) + 1) + " of " + str(len(WZ_bag_files)) + ")") - sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - bag = rosbag.Bag(bag_file_path) - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - - # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test - # TODO update to remove geofence information - print("Getting engagement times at " + str(datetime.datetime.now())) - # TODO get engagement times - #time_test_start_engagement, time_test_end_engagement, found_test_times = get_test_case_engagement_times(bag) - print("Got engagement times at " + str(datetime.datetime.now())) - found_test_times = True # TODO remove these placeholder values - time_test_start_engagement = 0.0 - time_test_end_engagement = 1.0 - if (not found_test_times): - print("Could not find test case engagement start and end times in bag file.") - continue - - # Debug Statements - print("Engagement starts at " + str(time_test_start_engagement.to_sec())) - print("Engagement ends at " + str(time_test_end_engagement.to_sec())) - print("Time spent engaged: " + str((time_test_end_engagement - time_test_start_engagement).to_sec()) + " seconds") - - # TODO implement - original_speed_limit = 1.0 # TODO remove placeholder - # original_speed_limit = get_route_original_speed_limit(bag, time_test_start_engagement) # Units: m/s - print("Original Speed Limit is " + str(original_speed_limit) + " m/s") - - # Initialize results - # TODO updates results - wz_1_result = None - wz_2_result = None - wz_3_result = None - wz_4_result = None - wz_5_result = None - wz_6_result = None - wz_7_result = None - wz_8_result = None - wz_9_result = None - wz_10_result = None - wz_11_result = None - wz_12_result = None - wz_13_result = None - wz_14_result = None - wz_15_result = None - wz_16_result = None - wz_17_result = None - wz_18_result = None - wz_19_result = None - wz_20_result = None - wz_21_result = None - wz_22_result = None - wz_23_result = None - wz_24_result = None - wz_25_result = None - wz_26_result = None - wz_27_result = None - - # TODO call metric functions - - # Write simple pass/fail results to .csv file for appropriate row: - csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, - wz_1_result, wz_2_result, wz_3_result, wz_4_result, wz_5_result, wz_6_result, wz_7_result, - wz_8_result, wz_9_result, wz_10_result, wz_11_result, wz_12_result, wz_13_result, wz_14_result, - wz_15_result, wz_16_result, wz_17_result, wz_18_result, wz_19_result, wz_20_result, - wz_21_result, wz_22_result, wz_23_result, wz_24_result, wz_25_result]) - - sys.stdout = orig_stdout - text_log_file_writer.close() - return - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_voices_sit1_rosbags.py b/engineering_tools/data_processing/analyze_voices_sit1_rosbags.py deleted file mode 100644 index 3ac1f8b893..0000000000 --- a/engineering_tools/data_processing/analyze_voices_sit1_rosbags.py +++ /dev/null @@ -1,776 +0,0 @@ -#!/usr/bin/python3 - -# 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. - -from inspect import TPFLAGS_IS_ABSTRACT -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime -import math - -# HOW TO USE SCRIPT: -# Run the following in a terminal: -# sudo add-apt-repository ppa:deadsnakes/ppa -# sudo apt-get update -# sudo apt install python3.7 -# python3.7 -m pip install --upgrade pip -# python3.7 -m pip install matplotlib -# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg -# python3.7 -m pip install lz4 -# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ -# In terminal, navigate to the directory that contains this python script and run the following: -# python3.7 analyze_voices_sit1_rosbags.py - -def generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_starting_vehicle_start_engagement, vehicle_1_platooning_times, vehicle_2_platooning_times, vehicle_3_platooning_times, test_type, test_num): - # vehicle 2 and 3 platooning times: [time_veh1_engaged, time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] - # vehicle 1 platooning times: [time_began_platooning_with_veh2, time_began_platooning_with_veh3, time_leader_end_engagement] - - if test_type == "VOICES-SIT1": - # First vehicle to engage is vehicle 1 - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - vehicle_1_speed_times = [] - vehicle_1_speeds = [] - for topic, msg, t in vehicle_1_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_starting_vehicle_start_engagement, end_time = vehicle_1_platooning_times[2]): # time_start_engagement+time_duration): - if first: - time_1_start = t - first = False - continue - - vehicle_1_speed_times.append((t-time_1_start).to_sec()) - vehicle_1_speeds.append(msg.twist.linear.x) # Current speed in m/s - - first = True - vehicle_2_speed_times = [] - vehicle_2_speeds = [] - for topic, msg, t in vehicle_2_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = vehicle_2_platooning_times[0], end_time = vehicle_2_platooning_times[3]): # time_start_engagement+time_duration): - if first: - time_2_start = t - first = False - continue - - vehicle_2_speed_times.append((t-time_2_start).to_sec()) - vehicle_2_speeds.append(msg.twist.linear.x) # Current speed in m/s - - first = True - vehicle_3_speed_times = [] - vehicle_3_speeds = [] - for topic, msg, t in vehicle_3_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = vehicle_3_platooning_times[0], end_time = vehicle_3_platooning_times[3]): # time_start_engagement+time_duration): - if first: - time_3_start = t - first = False - continue - - vehicle_3_speed_times.append((t-time_3_start).to_sec()) - vehicle_3_speeds.append(msg.twist.linear.x) # Current speed in m/s - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot vehicle 1 speed (m/s) vs. time - ax.plot(vehicle_1_speed_times, vehicle_1_speeds, 'g:', label='Vehicle 1 Speed (m/s)') - - # Plot vehicle 2 speed (m/s) vs. time - ax.plot(vehicle_2_speed_times, vehicle_2_speeds, 'b:', label='Vehicle 2 Speed (m/s)') - - # Plot vehicle 2 speed (m/s) vs. time - ax.plot(vehicle_3_speed_times, vehicle_3_speeds, 'r:', label='Vehicle 3 Speed (m/s)') - - # Optional: Plot a vertical bar at the time that vehicle 2 joins the platoon - #ax.axvline(x = (vehicle_2_platooning_times[2] - time_2_start).to_sec(), color = 'b', label = 'Time Vehicle 2 Joined the Platoon') - - # Optional: Plot a vertical bar at the time that vehicle 3 joins the platoon - #ax.axvline(x = (vehicle_3_platooning_times[2] - time_3_start).to_sec(), color = 'r', label = 'Time Vehicle 3 Joined the Platoon') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(test_type) + " Run " + str(test_num) + " Vehicle Speeds") # Plot Title - ax.set_xlabel("Time (seconds) Since Lead Vehicle Became Engaged") # Plot X Title - ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title - - # Option 1: Save the plot - filename = str(test_type) + "_Run_" + str(test_num) + "_Vehicle_Speeds.png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -def generate_speed_plot(bag, time_start_engagement, time_end_engagement, bag_file_name, speed_limit_zone_change_times, test_type, vehicle_number): - # Speed command: /hardware_interface/arbitrated_speed_commands: msg.speed (m/s) - # True Speed: /hardware_interface/vehicle/twist: msg.twist.linear.x (m/s) - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - true_vehicle_speed_times = [] - true_vehicle_speeds = [] - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - true_vehicle_speed_times.append((t-time_start).to_sec()) - true_vehicle_speeds.append(msg.twist.linear.x) # Current speed in m/s - - # TODO: UPDATE THE TOPIC USED FOR THE LEXUS/PACMOD SPEED COMMANDS - # Get the commanded vehicle speed (m/s) and the associated time with each data point - first = True - cmd_vehicle_speed_times = [] - cmd_vehicle_speeds = [] - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/arbitrated_speed_commands'], start_time = time_start_engagement, end_time = time_end_engagement): # time_start_engagement+time_duration): - if first: - time_start = t - first = False - continue - - cmd_vehicle_speed_times.append((t-time_start).to_sec()) - cmd_vehicle_speeds.append(msg.speed) # Commanded speed in m/s - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot commanded vehicle speed (m/s) vs. time - ax.plot(cmd_vehicle_speed_times, cmd_vehicle_speeds, 'g:', label='Commanded Speed (m/s)') - - # Plot true vehicle speed (m/s) vs. time - ax.plot(true_vehicle_speed_times, true_vehicle_speeds, 'b--', label='Actual Speed (m/s)') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Speed (Commanded and Actual) -- Vehicle " + str(vehicle_number)) # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Engagement") # Plot X Title - ax.set_ylabel("Vehicle Speed (m/s)") # Plot Y Title - - # Option 1: Save the plot - filename = str(test_type) + "_" + bag_file_name + ".png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -def generate_platooning_plot(bag, time_start_platooning, time_end_platooning, bag_file_name, test_type, vehicle_number): - - actual_gaps = [] - desired_gaps = [] - times = [] - first = True - for topic, msg, t in bag.read_messages(topics=['/guidance/platooning_info'], start_time = time_start_platooning, end_time = time_end_platooning): - if first: - time_start = t - first = False - continue - - if msg.actual_gap < 150 and msg.actual_gap > -150: - actual_gaps.append(msg.actual_gap) - desired_gaps.append(msg.desired_gap) - times.append((t-time_start).to_sec()) - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot actual gaps (meters) vs. time - ax.plot(times, actual_gaps, 'g:', label='Actual Gap (meters)') - - # Plot desired gaps (meters) vs. time - ax.plot(times, desired_gaps, 'r--', label='Desired Gap (meters)') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Platooning Information -- Vehicle " + str(vehicle_number)) # Plot Title - ax.set_xlabel("Time (seconds) Since Joining Platoon") # Plot X Title - ax.set_ylabel("Meters") # Plot Y Title - - # Option 1: Save the plot - filename = str(test_type) + "_Platooning_Information_" + bag_file_name + ".png" - plt.savefig(filename, bbox_inches='tight') - plt.close() - - # Option 2: Display the plot - #plt.show() - - return - -def generate_crosstrack_plot(bag, time_start_engagement, time_end_engagement, bag_file_name): - # Crosstrack Error: /message/route_state (meters)) - - # Get the true vehicle speed (m/s) and the associated time with each data point - first = True - crosstrack_times = [] - crosstrack_values = [] - # Note: This topic name assumes a pacmod controller is being used (freightliners or lexus) - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement, end_time = time_end_engagement): - if first: - time_start = t - first = False - continue - - crosstrack_times.append((t-time_start).to_sec()) - crosstrack_values.append(msg.cross_track) # Crosstrack error in meters - - # Create the initial plot with the defined figure size - fig, ax = plt.subplots(figsize=(9,5.5)) - - # Plot crosstrack error (meters) vs. time - ax.plot(crosstrack_times, crosstrack_values, 'g:', label='Crosstrack Error (meters)') - - # Optional: Plot a horizontal bar at a positive value for reference - ax.axhline(y = 0.95, color = 'r', label = '+/- 0.95 Meters (For Reference)') - - # Optional: Plot a horizontal bar at a negative value for reference - ax.axhline(y = -0.95, color = 'r') - - plt.rc('axes', labelsize=12) # fontsize of the axes labels - plt.rc('legend', fontsize=10) # fontsize of the legend text - ax.legend(loc = 'lower right') # Location of the legend - ax.set_title(str(bag_file_name) + " Crosstrack Error") # Plot Title - ax.set_xlabel("Time (seconds) Since Start of Engagement") # Plot X Title - ax.set_ylabel("Crosstrack Error (meters)") # Plot Y Title - - # Option 1: Save the plot - filename = "Crosstrack_" + bag_file_name + ".png" - #plt.savefig(filename, bbox_inches='tight') - #plt.close() - - # Option 2: Display the plot - plt.show() - - return - -# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section -def get_test_case_engagement_times(bag): - # Initialize system engagement start and end times - time_start_engagement = rospy.Time() - time_stop_engagement = rospy.Time() - - # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section - is_engaged = False - found_starting_engagement_time = False - found_ending_engagement_time = False - for topic, msg, t in bag.read_messages(topics=['/guidance/state']): - # If entering engagement, track this start time - if (msg.state == 4 and not is_engaged): - found_starting_engagement_time = True - time_start_engagement = t - is_engaged = True - - # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance - # state can be published. - if (msg.state == 4): - time_last_engaged = t - - # Log time that engagements ends - elif (msg.state != 4 and is_engaged): - found_ending_engagement_time = True - is_engaged = False - time_stop_engagement = t - - # If CARMA ended engagement before guidance state could be updated, update time_stop_engagement - if found_starting_engagement_time and not found_ending_engagement_time: - time_stop_engagement = time_last_engaged - - found_engagement_times = False - if found_starting_engagement_time: - found_engagement_times = True - - return time_start_engagement, time_stop_engagement, found_engagement_times - -def analyze_route_speed_limits(bag): - print("New route analyzed: ") - current_lanelet = None - current_speed_limit = None - lanelet_speed_limits = [] - speed_limit_zone_change_times = [] - speed_limit_zone_values = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): - if msg.lanelet_id != current_lanelet: - lanelet_speed_limits.append(msg.speed_limit) - current_lanelet = msg.lanelet_id - - if current_speed_limit is None: - print("New speed limit " + str(msg.speed_limit) + " at time " + str(t.to_sec())) - speed_limit_zone_values.append(msg.speed_limit) - speed_limit_zone_change_times.append(t) - current_speed_limit = msg.speed_limit - else: - if abs(current_speed_limit - msg.speed_limit) > 0.1: - print("New speed limit " + str(msg.speed_limit) + " at time " + str(t.to_sec())) - speed_limit_zone_values.append(msg.speed_limit) - speed_limit_zone_change_times.append(t) - current_speed_limit = msg.speed_limit - - # Remove first element for zero speed limit - lanelet_speed_limits = lanelet_speed_limits[1:] - speed_limit_zone_change_times = speed_limit_zone_change_times[1:] - speed_limit_zone_values = speed_limit_zone_values[1:] - - print("Route speed limits in m/s (1 entry for each shortest-path lanelet in order): " + str(lanelet_speed_limits[1:])) - - return speed_limit_zone_values,speed_limit_zone_change_times - - -def leader_platoon_formation_analysis(bag, run_type, leader_id, second_vehicle_id, third_vehicle_id, time_leader_end_engagement): - - if run_type == "VOICES-SIT1": - has_began_platooning_with_veh2 = False - time_began_platooning_with_veh2 = rospy.Time() - has_began_platooning_with_veh3 = False - time_began_platooning_with_veh3 = rospy.Time() - ack_num = 0 - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_response']): - if msg.header.sender_id == leader_id and msg.header.recipient_id == second_vehicle_id: - if msg.is_accepted: - ack_num += 1 - - if ack_num == 2: - has_began_platooning_with_veh2 = True - time_began_platooning_with_veh2 = t - print("Leader began platooning with veh2 at " + str(t.to_sec())) - elif msg.header.sender_id == leader_id and msg.header.recipient_id == third_vehicle_id: - if msg.is_accepted: - ack_num += 1 - - if ack_num == 4: - has_began_platooning_with_veh3 = True - time_began_platooning_with_veh3 = t - print("Leader began platooning with veh3 at " + str(t.to_sec())) - break - - - return [time_began_platooning_with_veh2, time_began_platooning_with_veh3, time_leader_end_engagement] - -def joiner_platoon_formation_analysis(bag, run_type, leader_id, follower_id, joiner_vehicle_number): - - has_sent_initial_request = False - time_sent_initial_request = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request']): - if msg.header.recipient_id == leader_id and msg.header.sender_id == follower_id: - if run_type == "VOICES-SIT1": - if msg.plan_type.type == 3: - has_sent_initial_request = True - time_sent_initial_request = t - print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " sent initial request at " + str(t.to_sec())) - - has_received_initial_ack = False - time_received_initial_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response']): - if msg.header.recipient_id == follower_id and msg.header.sender_id == leader_id: - if run_type == "VOICES-SIT1": - #if msg.plan_type.type == 3 and msg.is_accepted: - if msg.is_accepted: - has_received_initial_ack = True - time_received_initial_ack = t - print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " received ACK at " + str(t.to_sec())) - break - - has_sent_second_request = False - time_sent_second_request = rospy.Time() - first = True - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_mobility_request']): - if msg.header.recipient_id == leader_id and msg.header.sender_id == follower_id: - if run_type == "VOICES-SIT1": - if msg.plan_type.type == 4: - if first: - has_sent_second_request = True - time_sent_second_request = t - first = False - print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " sent second request at " + str(t.to_sec())) - - has_received_second_ack = False - time_received_second_ack = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_mobility_response']): - if msg.header.recipient_id == follower_id and msg.header.sender_id == leader_id: - if run_type == "VOICES-SIT1": - if msg.is_accepted: - has_received_second_ack = True - time_received_second_ack = t - print("DEBUG: Vehicle " + str(joiner_vehicle_number) + " received ACK at " + str(t.to_sec())) - - total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() - #print("DEBUG: Time between sending initial request and receiving ack: " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - #print("DEBUG: Time between sending second request and receiving ack: " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec") - #print("DEBUG: Time between sending initial request and receiving second ack: " + str(total_formation_duration) + " sec") - - if run_type == "VOICES-SIT1": - total_formation_duration = (time_received_second_ack - time_sent_initial_request).to_sec() - if joiner_vehicle_number == 2: - if has_received_initial_ack: - print("IHP2-1 Succeeded - Vehicle 2 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-1 Failed - Vehicle 2 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") - - if has_received_second_ack: - print("IHP2-3 Succeeded - Vehicle 2 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-3 Failed - Vehicle 2 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-2 Succeeded: Time between Vehicle 2 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack] - - elif joiner_vehicle_number == 3: - if has_received_initial_ack: - print("IHP2-7 Succeeded - Vehicle 3 received initial ACK from leader for JOIN_PLATOON_AT_REAR at " + str(time_received_initial_ack.to_sec()) + " in " + str((time_received_initial_ack - time_sent_initial_request).to_sec()) + " sec") - else: - print("IHP2-7 Failed - Vehicle 3 did not receive initial ACK from leader for JOIN_PLATOON_AT_REAR") - - if has_received_second_ack: - print("IHP2-9 Succeeded - Vehicle 3 received second ACK from leader for PLATOON_FOLLOWER_JOIN and joins platoon at " + str(time_received_second_ack.to_sec()) + " in " + str((time_received_second_ack - time_sent_second_request).to_sec()) + " sec; " + str((time_received_second_ack-time_received_initial_ack).to_sec()) + " sec after prev ack") - else: - print("IHP2-9 Failed - Vehicle 3 did not receive second ACK from leader for PLATOON_FOLLOWER_JOIN and did not join the platoon") - - if total_formation_duration <= 2.0: - print("IHP2-8 Succeeded: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - else: - print("IHP2-8 Failed: Time between Vehicle 3 sending initial request and receiving second ACK was " + str(total_formation_duration) + " sec") - - return [time_received_initial_ack, time_received_second_ack] - -def received_tcm_analysis(bag, time_start_platooning, test_type, vehicle_number, expected_tcm_count): - - # List to store the receive times of each TCM msgnum [time_receive_msgnum1, time_receive_msgnum2, etc.] - tcm_receive_times = [None for i in range(0, expected_tcm_count)] - - # Store all transmitted TCR reqids so that only corresponding received TCMs are analyzed - tcr_reqids = [] - tcr_reqids_times = [] - for topic, msg, t in bag.read_messages(topics=['/message/outgoing_geofence_request']): - tcr_reqids.append(msg.tcr_v01.reqid.id) - tcr_reqids_times.append(t) - - # Only analyze TCMs with reqids matching a transmitted TCR reqid. Populate the tcm_receive_times list - has_received_any_tcms = False - has_received_all_tcms = False - has_received_correct_tcms = True - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - reqid = msg.tcm_v01.reqid.id - if reqid in tcr_reqids: - has_received_any_tcms = True - msgnum = int(msg.tcm_v01.msgnum) - 1 - if tcm_receive_times[msgnum] is None: - time_received_after_platoon_start = (t-time_start_platooning).to_sec() - tcm_receive_times[msgnum] = time_received_after_platoon_start - - tcm_type = msg.tcm_v01.params.detail.choice - - if tcm_type == 12: - max_speed = msg.tcm_v01.params.detail.maxspeed - print("DEBUG: Received TCM msgnum " + str(msgnum) + " of " + str(msg.tcm_v01.msgtot) + " with type " + str(tcm_type) + " " + str(time_received_after_platoon_start) + " sec after platooning -- max speed " + str(max_speed)) - else: - print("Received non-maxspeed TCM") - - if None not in tcm_receive_times: - has_received_all_tcms = True - break - - if not has_received_any_tcms: - print("IHP2-4 Failed for vehicle " + str(vehicle_number) + ": No TCMs were received matching its TCR reqids") - print("IHP2-5 Failed for vehicle " + str(vehicle_number) + ": No TCMs were received matching its TCR reqids") - return - - #if has_received_all_tcms: - # time_received_first_tcm = min(tcm_receive_times) - # print("DEBUG: Vehicle " + str(vehicle_number) + " received first applicable TCM " + str(time_received_first_tcm) + " sec after beginning platooning") - - if min(tcm_receive_times) > 0: - print("IHP2-4 Succeeded for vehicle " + str(vehicle_number) + ": all TCMs were received after the vehicle began platooning") - else: - print("IHP2-4 Failed for vehicle " + str(vehicle_number) + ": at least one TCM was received before vehicle began platooning") - - if has_received_all_tcms and has_received_correct_tcms: - print("IHP2-5 Succeeded: CMV received all correct TCMs for test type " + str(test_type)) - elif not has_received_correct_tcms and not has_received_correct_tcms: - print("IHP2-5 Failed: CMV didn't receive all TCMs and didn't receive correct TCMs for test type " + str(test_type)) - elif not has_received_correct_tcms: - print("IHP2-5 Failed: CMV did not receive all correct TCMs for test type " + str(test_type)) - elif not has_received_all_tcms: - print("IHP2-5 Failed: CMV did not receive all TCM msgnums for test type " + str(test_type)) - - -def crosstrack_distance_analysis(join_vehicle_rosbag, other_vehicle_rosbag, run_type, join_vehicle_number, other_vehicle_number, time): - # Get other vehicle cross track at provided time (negative is to the left; positive is to the right) - for topic, msg, t in other_vehicle_rosbag.read_messages(topics=['/guidance/route_state'], start_time = time): - other_vehicle_cross_track = msg.cross_track - break - - # Get joining vehicle pose at provided time (negative is to the left; positive is to the right) - for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/guidance/route_state'], start_time = time): - join_vehicle_cross_track = msg.cross_track - break - - total_cross_track = abs(other_vehicle_cross_track - join_vehicle_cross_track) - print("DEBUG: Join vehicle " + str(join_vehicle_number) + " crosstrack " + str(join_vehicle_cross_track) + ", veh " + str(other_vehicle_number) + " crosstrack " + str(other_vehicle_cross_track) + ", total " + str(total_cross_track) + " meters at time " + str(time.to_sec())) - - if run_type == "ALR": - if total_cross_track < 1.5: - print("IHP2-20 Succeeded: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FOLLOWER_JOIN") - else: - print("IHP2-20 Failed: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FOLLOWER_JOIN") - elif run_type == "ALF": - if total_cross_track < 1.5: - print("IHP2-22 Succeeded: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FRONT_JOIN") - else: - print("IHP2-22 Failed: crosstrack between joining vehicle " + str(join_vehicle_number) + " and other vehicle " + str(other_vehicle_number) + " was " + str(total_cross_track) + " meters (< 1.5 desired) at PLATOON_FRONT_JOIN") - - return total_cross_track - -def downtrack_distance_analysis(join_vehicle_rosbag, other_vehicle_rosbag, run_type, join_vehicle_number, other_vehicle_number, time): - # Get other vehicle pose at provided time - for topic, msg, t in other_vehicle_rosbag.read_messages(topics=['/localization/current_pose'], start_time = time): - other_vehicle_pose = [msg.pose.position.x, msg.pose.position.y] - break - - # Get joining vehicle pose at provided time - for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/localization/current_pose'], start_time = time): - join_vehicle_pose = [msg.pose.position.x, msg.pose.position.y] - break - - # Get joining vehicle speed - for topic, msg, t in join_vehicle_rosbag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time): - join_vehicle_speed = msg.twist.linear.x - break - - distance_between_vehicles = ((other_vehicle_pose[0] - join_vehicle_pose[0])**2 + (other_vehicle_pose[1] - join_vehicle_pose[1])**2) ** 0.5 - time_gap_between_vehicles = distance_between_vehicles / join_vehicle_speed - - print("DEBUG: Joining vehicle " + str(join_vehicle_number) + " speed is " + str(join_vehicle_speed) + " m/s") - print("DEBUG: Distance between joining vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " at time " + str(time.to_sec()) + " is " + str(distance_between_vehicles) + " meters") - print("DEBUG: Time Gap between joining vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " at time " + str(time.to_sec()) + " is " + str(time_gap_between_vehicles) + " seconds") - - if run_type == "VOICES-SIT1": - # Values are the same for same-lane front join and same-lane rear join - if distance_between_vehicles <= 90.0 or time_gap_between_vehicles <= 15.0: - print("IHP2-17 (SLR) Succeeded: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - else: - print("IHP2-17 (SLR) Failed: Downtrack between join vehicle " + str(join_vehicle_number) + " and vehicle " + str(other_vehicle_number) + " was " + str(distance_between_vehicles) + " meters (<=90 desired), time gap was " + str(time_gap_between_vehicles) + " seconds (<=15 desired) at JOIN_PLATOON_AT_REAR") - - return distance_between_vehicles - -# Main Function; run all tests from here -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - source_folder = sys.argv[1] - - # # Re-direct the output of print() to a specified .txt file: - # orig_stdout = sys.stdout - # current_time = datetime.datetime.now() - # text_log_filename = "Results_" + str(current_time) + ".txt" - # text_log_file_writer = open(text_log_filename, 'w') - # sys.stdout = text_log_file_writer - - # # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - # csv_results_filename = "Results_" + str(current_time) + ".csv" - # csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - # csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", - # "IHP2-1", "IHP2-2", "IHP2-3", "IHP2-4", "IHP2-5", - # "IHP2-6", "IHP2-7", "IHP2-8", "IHP2-9", "IHP2-10", - # "IHP2-11", "IHP2-12", "IHP2-13", "IHP2-14", "IHP2-15", - # "IHP2-16", "IHP2-17", "IHP2-18", "IHP2-19"]) - - # Constants - SILVER_LEXUS = "DOT-45243" - SPR = "CARMA-SPR" - AUG = "CARMA-AUG" - - bag_file = "_2022-07-28-18-35-40.bag" - bag_file_path = str(source_folder) + "/" + bag_file - bag = rosbag.Bag(bag_file_path) - num = 0 - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - print("***********************************************") - print(msg) - num += 1 - if num == 5: - break - - sit1_bag_files = [[["veh1_run2_2022-07-28-16-05-55-run2-veh1.bag", - "veh2_run2_2022-07-28-16-08-05.bag", - "veh3_run2_2022-07-28-16-07-02.bag"], - [SILVER_LEXUS, AUG, SPR]]] - - # Loop to conduct data analysis on each grouping of test run rosbags: - test_num = 0 - for test_case_files in sit1_bag_files: - test_num += 1 - print("*****************************************************************") - if test_case_files in sit1_bag_files: - test_type = "VOICES-SIT1" - print("VOICES - SIT1 Test Case") - else: - print("Unknown test run type being processed.") - continue - - # Store the rosbag and vehicle type for each vehicle in the lineup (Vehicle 1, Vehicle 2, Vehicle 3) - bag_files_list = test_case_files[0] - vehicle_types = test_case_files[1] - for file_number in range(0, len(bag_files_list)): - print("--") - bag_file = bag_files_list[file_number] - if file_number == 0: - vehicle_number = 1 - vehicle_1_rosbag_name = bag_files_list[file_number] - vehicle_1_type = vehicle_types[0] - elif file_number == 1: - vehicle_number = 2 - vehicle_2_rosbag_name = bag_files_list[file_number] - vehicle_2_type = vehicle_types[1] - elif file_number == 2: - vehicle_number = 3 - vehicle_3_rosbag_name = bag_files_list[file_number] - vehicle_3_type = vehicle_types[2] - - print("Processing new bag for vehicle " + str(file_number+1) + ": " + str(bag_file) + " for " + str(vehicle_types[file_number])) - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - # sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(sit1_bag_files.index(test_case_files) + 1) + " of " + str(len(sit1_bag_files)) + ")") - # sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - - if vehicle_number == 1: - vehicle_1_rosbag = rosbag.Bag(bag_file_path) - elif vehicle_number == 2: - vehicle_2_rosbag = rosbag.Bag(bag_file_path) - elif vehicle_number == 3: - vehicle_3_rosbag = rosbag.Bag(bag_file_path) - - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test - print("Getting engagement times at " + str(datetime.datetime.now())) - if vehicle_number == 1: - time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_1_rosbag) - time_1_start_engagement = time_start_engagement - time_1_end_engagement = time_end_engagement - if vehicle_number == 2: - time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_2_rosbag) - time_2_start_engagement = time_start_engagement - time_2_end_engagement = time_end_engagement - if vehicle_number == 3: - time_start_engagement, time_end_engagement, found_test_times = get_test_case_engagement_times(vehicle_3_rosbag) - time_3_start_engagement = time_start_engagement - time_3_end_engagement = time_end_engagement - print("Got engagement times at " + str(datetime.datetime.now())) - - if (not found_test_times): - print("Could not find test case engagement start and end times in bag file.") - #continue - else: - print("Began engagement at " + str(time_start_engagement.to_sec())) - print("Ended engagement at " + str(time_end_engagement.to_sec())) - print("Time spent engaged: " + str((time_end_engagement - time_start_engagement).to_sec()) + " seconds") - - # if vehicle_number == 1: - # speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_1_rosbag) - # veh_1_speed_limit_zone_change_times = speed_limit_zone_change_times - # #generate_speed_plot(vehicle_1_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) - # #generate_crosstrack_plot(vehicle_1_rosbag, time_start_engagement, time_end_engagement, bag_file) - # if vehicle_number == 2: - # speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_2_rosbag) - # #generate_speed_plot(vehicle_2_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) - # #generate_crosstrack_plot(vehicle_2_rosbag, time_start_engagement, time_end_engagement, bag_file) - # if vehicle_number == 3: - # speed_limit_zone_values, speed_limit_zone_change_times = analyze_route_speed_limits(vehicle_3_rosbag) - # #generate_speed_plot(vehicle_3_rosbag, time_start_engagement, time_end_engagement, bag_file, speed_limit_zone_change_times, test_type, vehicle_number) - # #generate_crosstrack_plot(vehicle_3_rosbag, time_start_engagement, time_end_engagement, bag_file) - - if test_type == "VOICES-SIT1": - # Vehicle 2 joins Vehicle 1 from rear; then Vehicle 3 joins 1/2 from Rear - - # Platoon Formation - Joiner Perspective (IHP2 metrics 1,2,3,7,8,9) - vehicle_2_platooning_times = joiner_platoon_formation_analysis(vehicle_2_rosbag, test_type, vehicle_1_type, vehicle_2_type, 2) # [time_received_initial_ack, time_received_second_ack] - vehicle_3_platooning_times = joiner_platoon_formation_analysis(vehicle_3_rosbag, test_type, vehicle_1_type, vehicle_3_type, 3) # [time_received_initial_ack, time_received_second_ack] - vehicle_1_platooning_times = leader_platoon_formation_analysis(vehicle_1_rosbag, test_type, vehicle_1_type, vehicle_2_type, vehicle_3_type, time_1_end_engagement) # [time_began_platooning_with_veh2, time_began_platooning_with_veh3, time_leader_end_engagement] - - vehicle_1_platoon_duration = (vehicle_1_platooning_times[2] - vehicle_1_platooning_times[0]) - vehicle_2_platoon_duration = (vehicle_1_platooning_times[2] - vehicle_1_platooning_times[0]) - vehicle_3_platoon_duration = (vehicle_1_platooning_times[2] - vehicle_1_platooning_times[1]) - vehicle_2_platooning_times.append(vehicle_2_platooning_times[1] + vehicle_2_platoon_duration) # Now [time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] - vehicle_3_platooning_times.append(vehicle_3_platooning_times[1] + vehicle_3_platoon_duration) # Now [time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] - - - print("Vehicle 1 platoon duration: " + str(vehicle_1_platoon_duration.to_sec())) - print("Vehicle 2 platoon duration: " + str(vehicle_2_platoon_duration.to_sec())) - print("Vehicle 3 platoon duration: " + str(vehicle_3_platoon_duration.to_sec())) - - print("DEBUG: Vehicle 1 Platoon Start Time: " + str(vehicle_1_platooning_times[1].to_sec())) - print("DEBUG: Vehicle 1 Platoon End Time: " + str(vehicle_1_platooning_times[2].to_sec())) - - print("DEBUG: Vehicle 2 Platoon Start Time: " + str(vehicle_2_platooning_times[1].to_sec())) - print("DEBUG: Vehicle 2 Platoon End Time: " + str(vehicle_2_platooning_times[2].to_sec())) - - print("DEBUG: Vehicle 3 Platoon Start Time: " + str(vehicle_3_platooning_times[1].to_sec())) - print("DEBUG: Vehicle 3 Platoon End Time: " + str(vehicle_3_platooning_times[2].to_sec())) - - vehicle_1_engagement_duration = time_1_end_engagement - time_1_start_engagement - vehicle_2_platooning_times.insert(0, vehicle_2_platooning_times[2] - vehicle_1_engagement_duration) # Now [time_veh1_engaged, time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] - vehicle_3_platooning_times.insert(0, vehicle_3_platooning_times[2] - vehicle_1_engagement_duration) # Now [time_veh1_engaged, time_received_initial_ack, time_received_second_ack, time_veh1_left_platoon] - - print("Vehicle 1: " + str(vehicle_1_platooning_times)) - print("Vehicle 2: " + str(vehicle_2_platooning_times)) - print("Vehicle 3: " + str(vehicle_3_platooning_times)) - - #received_tcm_analysis(vehicle_1_rosbag, time_vehicle_1_platooning_start, test_type, 1, expected_tcm_count=7) - #received_tcm_analysis(vehicle_2_rosbag, vehicle_2_platooning_times[1], test_type, 2, expected_tcm_count=7) - #received_tcm_analysis(vehicle_3_rosbag, vehicle_3_platooning_times[1], test_type, 3, expected_tcm_count=7) - - print("IHP2-6 N/A for SLR Test; no platoon-specific TCMs") - print("IHP2-10 N/A for SLR Test; TCM speed limit matches original speed limit") - print("IHP2-11 N/A for SLR Test; no headway-specific TCMs") - print("IHP2-12 N/A for SLR Test; TCM speed limit matches original speed limit") - print("IHP2-13 N/A for SLR Test; no headway-specific TCMs") - - #downtrack_distance_analysis(vehicle_2_rosbag, vehicle_1_rosbag, test_type, 2, 1, vehicle_2_platooning_times[0]) - #downtrack_distance_analysis(vehicle_3_rosbag, vehicle_2_rosbag, test_type, 3, 2, vehicle_3_platooning_times[0]) - - generate_platooning_plot(vehicle_2_rosbag, vehicle_2_platooning_times[2], vehicle_2_platooning_times[3], vehicle_2_rosbag_name, test_type, 2) - generate_platooning_plot(vehicle_3_rosbag, vehicle_3_platooning_times[2], vehicle_3_platooning_times[3], vehicle_3_rosbag_name, test_type, 3) - - # Get vehicle 2 time when vehicle 1 engages - - # Get vehicle 3 time when vehicle 1 engages - - generate_three_vehicle_speed_plot(vehicle_1_rosbag, vehicle_2_rosbag, vehicle_3_rosbag, time_1_start_engagement, vehicle_1_platooning_times, vehicle_2_platooning_times, vehicle_3_platooning_times, test_type, test_num=2) - - print("Analysis complete") - - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/engineering_tools/data_processing/analyze_wz_rosbags.py b/engineering_tools/data_processing/analyze_wz_rosbags.py deleted file mode 100644 index ca03063b63..0000000000 --- a/engineering_tools/data_processing/analyze_wz_rosbags.py +++ /dev/null @@ -1,1299 +0,0 @@ -#!/usr/bin/python3 - -# Copyright (C) 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. - -from inspect import TPFLAGS_IS_ABSTRACT -import sys -import csv -import matplotlib.pyplot as plt -import rospy -import rosbag # To import this, run the following command: "pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg" -import datetime -import math - -# HOW TO USE SCRIPT: -# Run the following in a terminal to download dependencies: -# sudo add-apt-repository ppa:deadsnakes/ppa -# sudo apt-get update -# sudo apt install python3.7 -# python3.7 -m pip install --upgrade pip -# python3.7 -m pip install matplotlib -# python3.7 -m pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag rospkg -# python3.7 -m pip install lz4 -# python3.7 -m pip install roslz4 --extra-index-url https://rospypi.github.io/simple/ -# In terminal, navigate to the directory that contains this python script and run the following: -# python3.7 analyze_wz_rosbags.py - -def generate_speed_plot(bag): - # Get the vehicle speed and plot it - speed_received_first_msg = 0.0 - first = True - times = [] - speeds = [] - crosstracks = [] - downtracks = [] - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status']): - #for topic, msg, t in bag.read_messages(topics=['/guidance/route_state']): - if first: - time_start = t - first = False - continue - - times.append((t-time_start).to_sec()) - speeds.append(msg.speed * 0.621371) # Conversion from kph to mph - #crosstracks.append(msg.cross_track) - #downtracks.append(msg.down_track) - - plt.plot(times,speeds) - #plt.plot(times,crosstracks) - #plt.plot(times,downtracks) - plt.show() - - return - - -# Helper Function: Get times associated with the system entering the geofence and exiting the geofence -def get_geofence_entrance_and_exit_times(bag): - # Initialize geofence entrance and exit times - time_enter_active_geofence = rospy.Time() # Time that the vehicle has entered the geofence - time_exit_active_geofence = rospy.Time() # Time that the vehicle has exited the geofence-------------------------------------------------------------------------------------------------------- - - # Find geofence entrance and exit times - is_on_active_geofence = False - found_geofence_entrance_time = False - found_geofence_exit_time = False - for topic, msg, t in bag.read_messages(topics=['/environment/active_geofence']): - # If first occurrence of being in the active geofence, set the start time - if (msg.is_on_active_geofence and not is_on_active_geofence): - time_enter_active_geofence = t - #print("Entered geofence at " + str(t.to_sec())) - #print(msg) - found_geofence_entrance_time = True - is_on_active_geofence = True - - # If final occurrence of being in the active geofence, set the end time - if (not msg.is_on_active_geofence and is_on_active_geofence): - time_exit_active_geofence = t - found_geofence_exit_time = True - - time_in_geofence = (time_exit_active_geofence - time_enter_active_geofence).to_sec() - print("Spent " + str(time_in_geofence) + " sec in geofence. Started at " + str(time_enter_active_geofence.to_sec())) - is_on_active_geofence = False - #break - - # Check if both geofence start and end time were found - found_geofence_times = False - if (found_geofence_entrance_time and found_geofence_exit_time): - found_geofence_times = True - - return time_enter_active_geofence, time_exit_active_geofence, found_geofence_times - -# Helper Function: Get the original speed limit for the lanelets within the vehicle's route -# Note: Assumes that all lanelets in the route share the same speed limit prior to the first geofence CARMA Cloud message being processed. -def get_route_original_speed_limit(bag, time_test_start_engagement): - # Initialize the return variable - original_speed_limit = 0.0 - - # Find the speed limit associated with the first lanelet when the system first becomes engaged - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_test_start_engagement): - original_speed_limit = msg.speed_limit - break - - return original_speed_limit - -# Helper function: Begin with the time that the vehicle exits the active geofence according to -# /guidance/active_geofence topic. This helper function adjusts the time to be -# based on /guidance/route_state in order to be more accurate. -def adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit): - # Get the true time of the end of the geofence, based on when /guidance/route_state displays the - # original speed limit for the current vehicle location - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_exit_geofence): - if msg.speed_limit == original_speed_limit: - true_time_exit_geofence = t - break - - return true_time_exit_geofence - -# Helper Function: Get start and end times of the period of engagement that includes the in-geofence section -def get_test_case_engagement_times(bag, time_enter_active_geofence, time_exit_active_geofence): - # Initialize system engagement start and end times - time_start_engagement = rospy.Time() - time_stop_engagement = rospy.Time() - - # Loop through /guidance/state messages to determine start and end times of engagement that include the in-geofence section - is_engaged = False - found_engagement_times = False - has_reached_geofence_entrance = False - has_reached_geofence_exit = False - for topic, msg, t in bag.read_messages(topics=['/guidance/state']): - # If entering engagement, track this start time - if (msg.state == 4 and not is_engaged): - time_start_engagement = t - is_engaged = True - - # Store the last recorded engagement timestamp in case CARMA ends engagement before a new guidance - # state can be published. - if (msg.state == 4): - time_last_engaged = t - - # If exiting engagement, check whether this period of engagement included the geofence entrance and exit times - elif (msg.state != 4 and is_engaged): - is_engaged = False - time_stop_engagement = t - - # Check if the engagement start time was before the geofence entrance and exit times - if (time_start_engagement <= time_enter_active_geofence and t >= time_enter_active_geofence): - has_reached_geofence_entrance = True - if (time_start_engagement <= time_exit_active_geofence and t >= time_exit_active_geofence): - has_reached_geofence_exit = True - - # Set flag if this engagement period includes both the geofence entrance and exit times - if (has_reached_geofence_entrance and has_reached_geofence_exit): - found_test_case_engagement_times = True - break - - # If CARMA ended engagement before guidance state could be updated, check if the last recorded - # time of engagement came after exiting the geofence - if not found_engagement_times: - if time_last_engaged >= time_exit_active_geofence: - time_stop_engagement = time_last_engaged - found_engagement_times = True - - return time_start_engagement, time_stop_engagement, found_engagement_times - -# Helper Function: Get the lanelet IDs that are included in the geofence. -def get_geofence_lanelets(bag, time_start_engagement, advisory_speed_limit): - # Loop through route_state topic and add all unique - lanelets_in_geofence = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): - if (msg.speed_limit - advisory_speed_limit < 0.01): - if msg.lanelet_id not in lanelets_in_geofence: - lanelets_in_geofence.append(msg.lanelet_id) - - print("Lanelet IDs in the geofence: " + str(lanelets_in_geofence)) - - return lanelets_in_geofence - -# Helper Function: Get the route downtrack associated with the geofence entrance -def get_geofence_entrance_downtrack(bag, time_enter_geofence): - - # Return the first 'downtrack' published to /guidance/route_state after entering the geofence - downtrack_enter_geofence = 0.0 - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_enter_geofence): - downtrack_enter_geofence = msg.down_track - break - - print("Downtrack of geofence entrance: " + str(downtrack_enter_geofence) + " meters") - - return downtrack_enter_geofence - -# Helper Function: Print out the times associated with the vehicle entering each new lanelet according to /guidance/route_state -def print_lanelet_entrance_times(bag, time_start_engagement): - # Print out time vehicle enters each lanelet according to /guidance/route_state - id = 0 - print("/guidance/route_state lanelet change times:") - for topic, msg, t in bag.read_messages(topics=['/guidance/route_state'], start_time = time_start_engagement): - if msg.lanelet_id != id: - print("Time: " + str(t.to_sec()) + "; Lanelet: " + str(msg.lanelet_id) + "; Speed Limit: " + str(msg.speed_limit)) - id = msg.lanelet_id - - return - -########################################################################################################### -# Workzone WZ-1: The geofenced area is a part of the initial route plan. -# -# Workzone WZ-5: The vehicle receives a message from CC that includes the closed lane ahead. The vehicle -# processes this closed lane information. -########################################################################################################### -def check_geofence_in_initial_route(bag, closed_lanelets): - # Get each set route from the bag file (includes set routes and updated/re-rerouted routes) - shortest_path_lanelets = [] - for topic, msg, t in bag.read_messages(topics=['/guidance/route']): - # Print as Debug Statement - print("Shortest Path Route Update at " + str(t.to_sec()) + ": " + str(msg.shortest_path_lanelet_ids)) - - shortest_path_lanelets.append([]) - for lanelet_id in msg.shortest_path_lanelet_ids: - shortest_path_lanelets[-1].append(lanelet_id) - - # If there are two route paths, check that the first (original) route contains the closed lanelet(s) and the second route doesn't - # Note: Assumes there should be only two routes: (1) the initial route and (2) the re-routed route - initial_route_includes_closed_lane = False # Flag for B-1 success - map_is_updated_for_closed_lane = False # Flag for B-11 success - if (len(shortest_path_lanelets) == 2): - original_shortest_path = shortest_path_lanelets[0] - rerouted_shortest_path = shortest_path_lanelets[1] - - for lanelet_id in closed_lanelets: - if lanelet_id in original_shortest_path: - initial_route_includes_closed_lane = True - else: - initial_route_includes_closed_lane = False - break - - for lanelet_id in closed_lanelets: - if lanelet_id not in rerouted_shortest_path: - map_is_updated_for_closed_lane = True - else: - map_is_updated_for_closed_lane = False - break - else: - print("Invalid quantity of route updates found in bag file (" + str(len(shortest_path_lanelets)) + " found, 2 expected)") - - # Print result statements and return success flags - if (initial_route_includes_closed_lane): - print("WZ-1 succeeded; all closed lanelets " + str(closed_lanelets) + " were in the initial route") - else: - print("WZ-1 failed: not all closed lanelets " + str(closed_lanelets) + " were in the initial route.") - - if (map_is_updated_for_closed_lane): - print("WZ-5 succeeded: no closed lanelets " + str(closed_lanelets) + " were in the re-routed route.") - else: - print("WZ-5 failed: at least 1 closed lanelet " + str(closed_lanelets) + " was in the re-routed route.") - - return initial_route_includes_closed_lane, map_is_updated_for_closed_lane - -########################################################################################################### -# Workzone WZ-2: Amount of time that the vehicle is going at steady state (e.g. same lane, constant speed) -# before it receives the first CC message. (> 3 seconds) -########################################################################################################### -def check_steady_state_before_first_received_message(bag, time_start_engagement, time_received_first_message, original_speed_limit): - # (m/s) Threshold offset of vehicle speed to speed limit to be considered at steady state - threshold_speed_limit_offset = 0.89408 # 0.89408 m/s is 1 mph - min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset - max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset - - # (seconds) Minimum time between vehicle reaching steady state and first TIM MobilityOperation message being received - min_time_between_steady_state_and_msg = 10.0 - - # Get the time that the vehicle reaches within the set offset of the speed limit (while system is engaged) - time_start_steady_state = 0.0 - has_reached_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_start_engagement): - current_speed = msg.twist.linear.x # Current vehicle speed in m/s - if (max_steady_state_speed >= current_speed >= min_steady_state_speed): - has_reached_steady_state = True - time_start_steady_state = t - break - - # Check if the time the vehicle reaches steady state is more than 'min_time_between_steady_state_and_msg' seconds before the first received TIM message - if (has_reached_steady_state): - time_between_steady_state_and_msg = (time_received_first_message - time_start_steady_state).to_sec() - if (time_between_steady_state_and_msg >= min_time_between_steady_state_and_msg): - is_successful = True - print("WZ-2 succeeded; reached steady state " + str(time_between_steady_state_and_msg) + " seconds before receiving first TIM or TCM message.") - else: - is_successful = False - if (time_between_steady_state_and_msg > 0): - print("WZ-2 failed; reached steady state " + str(time_between_steady_state_and_msg) + " seconds before receiving first TIM or TCM message.") - else: - print("WZ-2 failed; reached steady state " + str(-time_between_steady_state_and_msg) + " seconds after receiving first TIM or TCM message.") - else: - print("WZ-2 failed; vehicle never reached steady state during rosbag recording.") - is_successful = False - - return is_successful - -########################################################################################################### -# Workzone WZ-4: The vehicle receives a message from CC that includes the new speed limit ahead. The vehicle -# processes this new speed limit. -# -# Workzone WZ-5: The vehicle receives a message from CC that includes the closed lane ahead. The vehicle -# processes this closed lane information. -# -# Workzone WZ-6: The vehicle receives a message from CC that includes the "taper right" closed lane ahead. -# The vehicle processes this closed lane information. -# -# Workzone WZ-7: The vehicle receives a message from CC that includes the "open right" closed lane ahead. -# The vehicle processes this closed lane information. -# -# Workzone WZ-8: The vehicle receives a message from CC that includes the "open right" closed lane ahead. -# The vehicle processes this closed lane information. -########################################################################################################### -def get_workzone_TCM_data(bag): - # Check that TCM Messages are received for closed lane, open right, taper right, direction reversal, and advisory speed - has_closed_lane = False - has_advisory_speed = False - has_direction_reversal = False - has_taper_right = False - has_open_right = False - advisory_speed = 0.0 - time_first_msg_received = rospy.Time() - first = True - for topic, msg, t in bag.read_messages(topics=['/message/incoming_geofence_control']): - if first: - time_first_msg_received = t - first = False - - if msg.tcmV01.params.detail.choice == 5 and msg.tcmV01.params.detail.closed == 1: - #print(msg) - has_closed_lane = True - elif msg.tcmV01.params.detail.choice == 5 and msg.tcmV01.params.detail.closed == 3: - latitude = msg.tcmV01.geometry.reflat - longitude = msg.tcmV01.geometry.reflon - print("Taper right has stop bar location at lat=" + str(latitude) + ", longitude=" + str(longitude)) - has_taper_right = True - elif msg.tcmV01.params.detail.choice == 5 and msg.tcmV01.params.detail.closed == 5: - #print(msg) - has_open_right = True - elif msg.tcmV01.params.detail.choice == 7 and msg.tcmV01.params.detail.direction == 1: - #print(msg) - has_direction_reversal = True - elif msg.tcmV01.params.detail.choice == 12: - #print(msg) - has_advisory_speed = True - advisory_speed = msg.tcmV01.params.detail.maxspeed - - if has_closed_lane and has_advisory_speed and has_direction_reversal and has_taper_right and has_open_right: - print("TCM Messages Received: Closed Lane; Direction Reversal; Taper Right; Open Right; Advisory Speed: " + str(advisory_speed) + \ - " mph)") - break - - wz_4_successful = False - if has_advisory_speed: - print("WZ-4 succeeded; TCM message received with advisory speed limit " + str(advisory_speed) + " mph") - wz_4_successful = True - else: - print("WZ-4 failed; no TCM message with an advisory speed limit was received.") - - wz_5_successful = False - if has_closed_lane: - print("WZ-5 succeeded; TCM message received with closed lane") - wz_5_successful = True - else: - print("WZ-5 failed; no TCM message with closed lane.") - - wz_6_successful = False - if has_taper_right: - print("WZ-6 succeeded; TCM message received with a taper right closed lane") - wz_6_successful = True - else: - print("WZ-6 failed; no TCM message with a taper right closed lane was received.") - - wz_7_successful = False - if has_open_right: - print("WZ-7 succeeded; TCM message received with an open right closed lane") - wz_7_successful = True - else: - print("WZ-7 failed; no TCM message with an open right closed lane was received.") - - wz_8_successful = False - if has_direction_reversal: - print("WZ-8 succeeded; TCM message received with a direction reversal") - wz_8_successful = True - else: - print("WZ-8 failed; no TCM message with a direction reversal was received.") - - return advisory_speed, time_first_msg_received, wz_4_successful, wz_5_successful, wz_6_successful, wz_7_successful, wz_8_successful - -########################################################################################################### -# Workzone WZ-9: The vehicle shall continuously receive SPAT messages with a time gap between sequentially -# received SPAT messages below 0.5 seconds at least 90% of the time while engaged. -########################################################################################################### -def check_percentage_successful_spat_msg(bag, time_start_engagement, time_end_engagement): - # Parameters used for the computation of this metric - min_percent_time_successful = 90.0 # (90%); Percent of active platooning time that the maximum or better 'SPAT' frequency must be achieved - max_duration_between_msgs = 0.50 # (Seconds); Maximum duration between sequentially broadcasted 'SPAT' messages - - # Obtain the quantity of SPAT messages received during engagement - duration_since_prev_msg = 0.0 - total_time_between_unsuccessful_msgs = 0.0 - total_time_between_successful_msgs = 0.0 - largest_duration_between_msgs = 0.0 - total_time = 0.0 - total_time_between_unsuccessful_msgs = 0.0 - count_msgs = 0 - count_msgs_above_max_duration = 0 - time_prev_msg = rospy.Time() - first = True - current_signal_state = 0 - for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_start_engagement, end_time = time_end_engagement): - if first: - time_prev_msg = t - first = False - continue - - for movement in msg.intersection_state_list: - signal_state = movement.movement_list[1].movement_event_list[0].event_state.movement_phase_state - if (signal_state != current_signal_state): - print("Transitioned from signal " + str(current_signal_state) + " to " + str(signal_state) + " at " + str(t.to_sec())) - current_signal_state = signal_state - #print("***************************") - #print(len(movement.movement_list)) - #print(str(count_msgs) + " msg has phase:") - #print(str(movement.movement_list[0].movement_event_list[0].event_state.movement_phase_state) + " for SG_ID " + str(movement.movement_list[0].signal_group)) - #print("SG_ID " + str(movement.movement_list[1].signal_group) + " has signal " + str(movement.movement_list[1].movement_event_list[0].event_state.movement_phase_state)) - - #for move in movement.movement_list: - # for event in move.movement_event_list: - # print(event.event_state.movement_phase_state) - # msg.intersection_state_list.movement_list.event_state.movement_phase_state - - - - duration_since_prev_msg = (t - time_prev_msg).to_sec() - time_prev_msg = t - #print("Duration between SPAT messages: " + str(duration_since_prev_msg) + " sec") - - # Track the number of messages received after the (1/min_frequency) duration just for debugging purposes - if (duration_since_prev_msg >= max_duration_between_msgs): - total_time_between_unsuccessful_msgs += duration_since_prev_msg - count_msgs_above_max_duration += 1 - else: - total_time_between_successful_msgs += duration_since_prev_msg - - if duration_since_prev_msg > largest_duration_between_msgs: - largest_duration_between_msgs = duration_since_prev_msg - - total_time += duration_since_prev_msg - count_msgs += 1 - - percent_time_successful = (total_time_between_successful_msgs / total_time) * 100.0 - - # Update is_successful flag and print debug statements - is_successful = False - if percent_time_successful >= min_percent_time_successful: - print("WZ-9 Succeeded; Time between SPAT messages was below " + str(round(max_duration_between_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = True - else: - print("WZ-9 Failed; Time between SPAT messages was below " + str(round(max_duration_between_msgs,3)) + " seconds " + str(round(percent_time_successful,3)) + \ - "% of the time. (Must be greater than " + str(round(min_percent_time_successful,3)) + "%)") - is_successful = False - - pct_above_max_duration = (float(count_msgs_above_max_duration) / float(count_msgs)) * 100.0 - print(str(count_msgs) + " Total SPAT Messages sent (" + str(round(100-pct_above_max_duration,3)) + "% Successful); " + str(count_msgs_above_max_duration) + " after " + str(round(max_duration_between_msgs,3)) + " sec (" \ - + str(round(pct_above_max_duration,3)) + "%);; Longest duration " \ - + " between SPAT messages was " + str(largest_duration_between_msgs) + " sec") - - return is_successful - -########################################################################################################### -# Workzone WZ-10: The vehicle shall never receive sequential SPAT messages with a time gap between messages -# that exceeds 1 second. -########################################################################################################### -def check_duration_between_spat_msg_below_max_duration(bag, time_start_engagement, time_end_engagement): - # Parameters used for the computation of this metric - max_duration_between_msgs = 1.00 # (Seconds); Max duration between sequentially broadcasted 'SPAT' messages - - # Obtain the quantity of SPAT messages received during engagement - duration_since_prev_msg = 0.0 - total_time_between_unsuccessful_msgs = 0.0 - total_time_between_successful_msgs = 0.0 - largest_duration_between_msgs = 0.0 - total_time = 0.0 - total_time_between_unsuccessful_msgs = 0.0 - count_msgs = 0 - count_msgs_above_max_duration = 0 - time_prev_msg = rospy.Time() - first = True - for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_start_engagement, end_time = time_end_engagement): - if first: - time_prev_msg = t - first = False - continue - - m = msg - duration_since_prev_msg = (t - time_prev_msg).to_sec() - time_prev_msg = t - #print("Duration between SPAT messages: " + str(duration_since_prev_msg) + " sec") - - # Track the number of messages received after the (1/min_frequency) duration just for debugging purposes - if (duration_since_prev_msg >= max_duration_between_msgs): - total_time_between_unsuccessful_msgs += duration_since_prev_msg - count_msgs_above_max_duration += 1 - print(str(duration_since_prev_msg) + " sec between SPAT messages") - else: - total_time_between_successful_msgs += duration_since_prev_msg - - if duration_since_prev_msg > largest_duration_between_msgs: - largest_duration_between_msgs = duration_since_prev_msg - - total_time += duration_since_prev_msg - count_msgs += 1 - #print(msg) - percent_time_successful = (total_time_between_successful_msgs / total_time) * 100.0 - - # Update is_successful flag and print debug statements - is_successful = False - if count_msgs_above_max_duration == 0: - print("WZ-10 Succeeded; " + str(count_msgs_above_max_duration) + " SPAT messages more than 1.0 sec apart") - is_successful = True - else: - print("WZ-10 Succeeded; " + str(count_msgs_above_max_duration) + " SPAT messages more than 1.0 sec apart") - is_successful = False - - return is_successful - -########################################################################################################### -# Workzone WZ-11: After the vehicle processes a received SPAT message and determines that it will arrive at -# a red or yellow signal indication, the vehicle's actual trajectory in preparation for a -# stop will include a deceleration section. The average deceleration over the entire section -# shall be no less than 1 m/s^2, and the average deceleration over any 1-second portion of -# the section shall be no greater than 2.0 m/s^2. -########################################################################################################### -def check_deceleration_for_red_light(bag, time_start_engagement): - # Obtain the timestamp at which the vehicle comes to a complete stop at the red light - time_stopped = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement): - if (t-time_start_engagement).to_sec() < 5.0: - continue - - if msg.speed < 0.10: - time_stopped = t - print("Vehicle came to complete stop " + str((time_stopped - time_start_engagement).to_sec()) + " seconds after engagement") - break - - # Verify that light indication is RED (signal phase 3) at time_stopped - one_second_duration = rospy.Duration(1.0) - for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_stopped - one_second_duration): - signal_at_stop = msg.intersection_state_list[0].movement_list[1].movement_event_list[0].event_state.movement_phase_state - - if (signal_at_stop == 3): - print("Vehicle came to a stop at a Red Signal Indication: " + str(signal_at_stop)) - elif (signal_at_stop == 6): - print("Vehicle came to a stop at a Green Signal Indication: " + str(signal_at_stop)) - elif (signal_at_stop == 8): - print("Vehicle came to a stop at a Yellow Signal Indication: " + str(signal_at_stop)) - - break - - # Obtain time_last_accel, which is the last acceleration before decelerating to a complete stop - prev_speed = 0.0 - first = True - time_last_accel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement, end_time = time_stopped-one_second_duration): - if first: - prev_speed = msg.speed - first = False - continue - - delta_speed = msg.speed - prev_speed - if delta_speed >= 0: - time_last_accel = t - speed_after_final_increase = msg.speed * 0.277777 # Conversion from kph to m/s - - prev_speed = msg.speed - - print("Final speed increase before stop occurred at " + str(time_last_accel.to_sec()) + ", " + str((time_last_accel-time_start_engagement).to_sec()) + " seconds after engagement") - print("Speed after final acceleration before decelerating to a stop was " + str(speed_after_final_increase) + " m/s ") - - # Obtain average decelation over all 1-second windows in deceleration section - all_one_second_windows_successful = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_last_accel, end_time = (time_stopped-one_second_duration)): - speed_initial = msg.speed * 0.277777 # Conversion from kph to m/s - time_initial = t - - speed_final = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): - speed_final = msg.speed * 0.277777 # Conversion from kph to m/s - t_final = t - break - - one_second_decel = (speed_initial - speed_final) / (t_final-time_initial).to_sec() - - if one_second_decel > 2.0: - print("Failure: 1-second window had decel rate was " + str(one_second_decel) + " m/s^2; end time was " + str((time_stopped - t_final).to_sec()) + " seconds before stopping") - all_one_second_windows_successful = False - #else: - # print("Success: 1-second window has decel rate at " + str(one_second_decel) + " m/s^2") - - # Obtain the average deceleration over the entire deceleration section - average_total_decel = speed_after_final_increase / (time_stopped-time_last_accel).to_sec() - - total_deceleration_rate_successful = False - if average_total_decel >= 1.0: - print("WZ-11 (total deceleration before red light stop) succeeded; total average deceleration was " + str(average_total_decel) + " m/s^2") - total_deceleration_rate_successful = True - else: - print("WZ-11 (total deceleration before red light stop) failed; total average deceleration was " + str(average_total_decel) + " m/s^2") - - if all_one_second_windows_successful: - print("WZ-11 (1-second window deceleration before red light stop) succeeded; no occurrences of 1-second average deceleration above 2.0 m/s^2") - else: - print("WZ-11 (1-second window deceleration before red light stop) failed; at least one occurrence of 1-second average deceleration above 2.0 m/s^2") - - is_successful = False - if total_deceleration_rate_successful and all_one_second_windows_successful: - is_successful = True - - return is_successful, time_last_accel, time_stopped - -def euclidean_distance(p1, p2): - distance = math.sqrt((p1[0]-p2[0])**2 + (p1[1] - p2[1])**2) - return distance - -########################################################################################################### -# Workzone WZ-12: After the vehicle processes a received SPAT message and determines that it will arrive at -# a red or yellow signal indication, the vehicle will come to a complete stop 0-15 feet before -# the corresponding stop bar for the specified traffic signal. -########################################################################################################### -def check_stop_location_for_red_light(bag, stop_bar_location, dist_rear_axle_to_front_bumper, time_start_engagement, time_start_decel, time_stopped): - # Obtain true stopping location in map frame - system_stop_location = [0,0] - for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_stopped): - system_stop_location[0] = msg.pose.position.x - system_stop_location[1] = msg.pose.position.y - print("Red Light: System stopped at map location x=" + str(system_stop_location[0]) + ", y=" + str(system_stop_location[1])) - break - - # Obtain the location of the system at the start of engagement - system_start_location = [0,0] - for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_start_engagement): - system_start_location[0] = msg.pose.position.x - system_start_location[1] = msg.pose.position.y - break - - dist_start_to_vehicle_stop = euclidean_distance(system_start_location, system_stop_location) - dist_start_to_stop_bar = euclidean_distance(system_start_location, stop_bar_location) - dist_vehicle_to_stop_bar = euclidean_distance(stop_bar_location, system_stop_location) - print("Distance start to stop: " + str(dist_start_to_vehicle_stop) + " meters") - print("Distance start to stop bar: " + str(dist_start_to_stop_bar) + " meters") - print("Distance stop bar to stop: " + str(dist_vehicle_to_stop_bar) + " meters") - - is_successful = False - if dist_start_to_vehicle_stop < dist_start_to_stop_bar: - dist_vehicle_to_stop_bar = dist_vehicle_to_stop_bar - dist_rear_axle_to_front_bumper - if dist_vehicle_to_stop_bar < 0: - print("WZ-12 Failed; Vehicle stopped with front bumper " + str(abs(dist_vehicle_to_stop_bar)) + " meters after stop bar") - elif dist_vehicle_to_stop_bar < 15.0: - print("WZ-12 Succeeded; Vehicle stopped with front bumper " + str(dist_vehicle_to_stop_bar) + " meters in front of stop bar") - is_successful = True - else: - print("WZ-12 Failed; Vehicle stopped with front bumper " + str(dist_vehicle_to_stop_bar) + " meters before stop bar") - else: - dist_vehicle_to_stop_bar = dist_vehicle_to_stop_bar + dist_rear_axle_to_front_bumper - print("WZ-12 Failed; Vehicle stopped with front bumper " + str(dist_vehicle_to_stop_bar) + " meters after stop bar") - - return is_successful - -########################################################################################################### -# Workzone WZ-13: If the vehicle is stopped for a red or yellow signal indication and receives a new SPAT -# message for a green signal indication, the vehicle will begin accelerating within 0-4 seconds. -########################################################################################################### -def check_acceleration_time_after_green_light(bag, time_stopped): - max_time_to_start_accel = 4.0 # (Seconds) - - # Get occurrence of time_start_green_light after coming to a stop - time_start_green_light = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_stopped): - signal_at_stop = msg.intersection_state_list[0].movement_list[1].movement_event_list[0].event_state.movement_phase_state - - if signal_at_stop == 6: - time_start_green_light = t - break - - # Get timestamp of first acceleration after time_start_green_lightm - time_start_accel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_green_light): - if msg.speed > 0.1: - time_start_accel = t - break - - time_to_start_accel = (time_start_accel-time_start_green_light).to_sec() - - is_successful = False - if time_to_start_accel <= max_time_to_start_accel: - print("WZ-13 Succeeded; after green light, system took " + str(time_to_start_accel) + " seconds to begin accelerating (less than 4)") - is_successful = True - else: - print("WZ-13 Succeeded; after green light, system took " + str(time_to_start_accel) + " seconds to begin accelerating (more than 4)") - - return is_successful - -########################################################################################################### -# Workzone WZ-14: If the vehicle is stopped for a red or yellow signal indication and receives a new SPAT -# message for a green signal indication, the actual trajectory back to normal operations -# will include an acceleration section. The average acceleration over the entire section -# shall be no less than 1 m/s^2, and the average acceleration over any 1-second portion of -# the section shall be no greater than 2.0 m/s^2. -########################################################################################################### -def check_acceleration_after_stop(bag, time_stopped): - # Obtain time_start_accel - time_start_accel = rospy.Time() - speed_start_accel = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_stopped): - if msg.speed >= 0.1: - speed_start_accel = msg.speed * 0.277778 # Conversion kph to m/s - time_start_accel = t - break - - # Obtain time_end_accel (first deceleration after starting accel; also print the speed at end of accel) - prev_speed = 0.0 - first = True - time_end_accel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_accel): - if first: - prev_speed = msg.speed - first = False - continue - - delta_speed = msg.speed - prev_speed - if delta_speed <= 0: - time_end_accel = t - speed_end_accel = msg.speed * 0.277778 # Conversion kph to m/s - speed_after_final_increase_mph = msg.speed * 0.621371 # Conversion from kph to mph - print("Speed at end of accel after light turns green: " + str(speed_after_final_increase_mph) + " mph") - break - - prev_speed = msg.speed - - # Check the total average - total_average_decel = (speed_end_accel - speed_start_accel) / (time_end_accel-time_start_accel).to_sec() - print("Total average decel: " + str(total_average_decel) + " m/s^2") - - # Check the 1-second window averages - # Obtain average decelation over all 1-second windows in acceleration section - one_second_duration = rospy.Duration(1.0) - all_one_second_windows_successful = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_accel, end_time = (time_end_accel-one_second_duration)): - speed_initial = msg.speed * 0.277777 # Conversion from kph to m/s - time_initial = t - - speed_final = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): - speed_final = msg.speed * 0.277777 # Conversion from kph to m/s - t_final = t - break - - one_second_decel = (speed_final - speed_initial) / (t_final-time_initial).to_sec() - - if one_second_decel > 2.0: - print("Failure: 1-second window had accel rate was " + str(one_second_decel) + " m/s^2; end time was " + str((time_stopped - t_final).to_sec()) + " seconds before stopping") - all_one_second_windows_successful = False - #else: - # print("Success: 1-second window has accel rate at " + str(one_second_decel) + " m/s^2") - - # Print success/failure statements - total_deceleration_rate_successful = False - if total_average_decel >= 1.0: - print("WZ-14 (total accel after complete stop) succeeded; total average acceleration was " + str(total_average_decel) + " m/s^2") - total_deceleration_rate_successful = True - else: - print("WZ-14 (total accel after complete stop) failed; total average acceleration was " + str(total_average_decel) + " m/s^2") - - if all_one_second_windows_successful: - print("WZ-14 (1-second accel after complete stop) succeeded; no occurrences of 1-second average acceleration above 2.0 m/s^2") - else: - print("WZ-14 (1-second accel after complete stop) failed; at least one occurrence of 1-second average acceleration above 2.0 m/s^2") - - is_successful = False - if total_deceleration_rate_successful and all_one_second_windows_successful: - is_successful = True - - return is_successful - - -########################################################################################################### -# Workzone WZ-15: If the vehicle processes a received SPAT message and determines that it will arrive at a -# green traffic signal indication, the vehicle shall travel past the traffic signal at a speed -# of +/- 2 mph of the speed limit while the signal indication is green. -########################################################################################################### -def check_vehicle_speed_at_green_light(bag, advisory_speed_limit, stop_bar_location, time_start_engagement): - # Get time_vehicle_at_stop_bar (which is the time it passes the stop bar line) - min_dist_to_stop_bar = 1.0 # meteres - vehicle_location = [0,0] - time_vehicle_at_stop_bar = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/localization/current_pose'], start_time = time_start_engagement): - vehicle_location[0] = msg.pose.position.x - vehicle_location[1] = msg.pose.position.y - if (euclidean_distance(vehicle_location, stop_bar_location) <= min_dist_to_stop_bar): - time_vehicle_at_stop_bar = t - break - - # Get traffic signal value at time_pass_green_light - has_passed_at_green_light = False - for topic, msg, t in bag.read_messages(topics=['/message/incoming_spat'], start_time = time_vehicle_at_stop_bar): - signal_at_stop = msg.intersection_state_list[0].movement_list[1].movement_event_list[0].event_state.movement_phase_state - - if (signal_at_stop == 3): - print("WZ-15 Failed; Vehicle passed light during a Red Signal Indication: ") - elif (signal_at_stop == 6): - print("WZ-15 succeeded; Vehicle passed light during a Green Signal Indication: ") - has_passed_at_green_light = True - elif (signal_at_stop == 8): - print("WZ-15 Failed; Vehicle passed light during a Yellow Signal Indication: ") - - break - - # Get vehicle speed at time_pass_green_light - - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_vehicle_at_stop_bar): - speed_at_light = msg.speed * 0.621371 # Conversion from kph to mph - break - - has_passed_green_light_at_correct_speed = False - original_speed_limit = advisory_speed_limit * 2.23694 # m/s to mph - min_speed = original_speed_limit - 2.0 - max_speed = original_speed_limit + 2.0 - if min_speed <= speed_at_light <= max_speed: - has_passed_at_green_light = True - print("WZ-15 succeeded; vehicle passed light at speed " + str(speed_at_light) + " mph") - else: - print("WZ-15 failed; vehicle passed light at speed " + str(speed_at_light) + " mph") - - - # Print success/failure statements - is_successful = False - if has_passed_at_green_light and has_passed_green_light_at_correct_speed: - is_successful = True - - return is_successful, time_vehicle_at_stop_bar - -########################################################################################################### -# Workzone WZ-16: The actual trajectory in preparation for the geofenced area with an advisory speed limit -# will include a deceleration section. The average deceleration over the entire section shall -# be no less than 1 m/s^2, and the average deceleration over any 1-second portion of the section -# shall be no greater than 2.0 m/s^2. -########################################################################################################### -def check_deceleration_after_green_light(bag, time_vehicle_at_stop_bar, time_start_engagement): - # Obtain the last speed decrease time before reaching stop bar - prev_speed = 0.0 - first = True - time_last_decel = rospy.Time() - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement, end_time = time_vehicle_at_stop_bar): - if first: - prev_speed = msg.speed - first = False - continue - - delta_speed = msg.speed - prev_speed - if delta_speed <= 0: - time_last_decel = t - speed_after_final_decel = msg.speed * 0.277777 # Conversion from kph to m/s - #print("Time " + str(t.to_sec()) + " has speed " + str(speed_after_final_decel)) - - prev_speed = msg.speed - - prev_speed = 0.0 - first = True - time_last_accel = rospy.Time() - one_second_duration = rospy.Duration(1.0) - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_start_engagement, end_time = time_last_decel - one_second_duration): - if first: - prev_speed = msg.speed - first = False - continue - - delta_speed = msg.speed - prev_speed - if delta_speed >= 0: - time_last_accel = t - speed_after_final_accel = msg.speed * 0.277777 # Conversion from kph to m/s - - prev_speed = msg.speed - - speed_after_final_accel_mph = speed_after_final_accel * 2.23694 - speed_after_final_decel_mph = speed_after_final_decel * 2.23694 - print("(WZ-16): Speed start of decel: " + str(speed_after_final_accel) + " m/s; " + str(speed_after_final_accel_mph) + " mph") - print("(WZ-16): Speed end of decel " + str(speed_after_final_decel) + " m/s; " + str(speed_after_final_decel_mph) + " mph") - speed_start_decel = speed_after_final_accel - speed_end_decel = speed_after_final_decel - - total_average_decel = (speed_start_decel - speed_end_decel) / (time_last_decel - time_last_accel).to_sec() - - - # Obtain average decelation over all 1-second windows in deceleration section - one_second_duration = rospy.Duration(1.0) - all_one_second_windows_successful = True - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = time_last_accel, end_time = (time_last_decel-one_second_duration)): - speed_initial = msg.speed * 0.277777 # Conversion from kph to m/s - time_initial = t - - speed_final = 0.0 - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle_status'], start_time = (time_initial+one_second_duration)): - speed_final = msg.speed * 0.277777 # Conversion from kph to m/s - t_final = t - break - - one_second_decel = (speed_initial - speed_final) / (t_final-time_initial).to_sec() - - if one_second_decel > 2.0: - print("Failure: 1-second window had decel rate was " + str(one_second_decel) + " m/s^2; end time was " + str((time_last_accel - t_final).to_sec()) + " seconds before end of deceleration") - all_one_second_windows_successful = False - #else: - # print("Success: 1-second window has decel rate at " + str(one_second_decel) + " m/s^2") - - # Print success/failure statements - total_deceleration_rate_successful = False - if total_average_decel >= 1.0: - print("WZ-16 (total decel for geofence) succeeded; total average deceleration was " + str(total_average_decel) + " m/s^2") - total_deceleration_rate_successful = True - else: - print("WZ-16 (total decel for geofence) failed; total average deceleration was " + str(total_average_decel) + " m/s^2") - - if all_one_second_windows_successful: - print("WZ-16 (1-second decel for geofence) succeeded; no occurrences of 1-second average deceleration above 2.0 m/s^2") - else: - print("WZ-16 (1-second decel for geofence) failed; at least one occurrence of 1-second average deceleration above 2.0 m/s^2") - - - is_successful = False - if total_deceleration_rate_successful and all_one_second_windows_successful: - is_successful = True - return is_successful - -########################################################################################################### -# Workzone WZ-17: When navigating the "taper right" closed lane section of the route, the vehicle will travel -# sequentially through lanelets 1302928, 1303118, and 1303305. -########################################################################################################### - -########################################################################################################### -# Workzone WZ-18: When navigating the "open right" closed lane section of the route, the vehicle will travel -# sequentially through lanelets 1303305, 1303304, and 1302937. -########################################################################################################### - -########################################################################################################### -# Workzone WZ-19: After exiting the geofenced area with an advisory speed limit, the vehicle will begin -# accelerating back to the original speed limit within 0-30 feet. -########################################################################################################### - -########################################################################################################### -# Workzone WZ-20: After exiting the geofenced area with an advisory speed limit, the actual trajectory back -# to normal operations will include an acceleration section. The average acceleration over -# the entire section shall be no less than 1 m/s^2, and the average acceleration over any -# 1-second portion of the section shall be no greater than 2.0 m/s^2. -########################################################################################################### - -########################################################################################################### -# Workzone WZ-21: When not changing lanes, the vehicle is fully contained within its lane (it does not cross -# either of the lane lines) for at least 95% of the time spent with the CARMA system engaged. -########################################################################################################### - -########################################################################################################### -# Workzone WZ-22: When not changing lanes, the vehicle never crosses the left or right lane line associated -# with its current lane by more than 0.1 meters while the CARMA system is engaged. -########################################################################################################### - -########################################################################################################### -# Workzone WZ-23: After exiting the geofenced area, system is at steady state for at least 3 seconds before -# the end of the use case. -########################################################################################################### -def check_steady_state_after_geofence(bag, time_end_engagement, original_speed_limit): - ten_second_duration = rospy.Duration(10.0) - - - # Parameters used for metric evaluation - # (m/s) Threshold offset from speed limit; vehicle considered at steady state when its speed is within this offset of the speed limit - threshold_speed_limit_offset = 0.894 # 0.894 m/s is 2 mph - # (m/s) Minimum speed to be considered at steady state - min_steady_state_speed = original_speed_limit - threshold_speed_limit_offset - # (m/s) Maximum speed to be considered at steady state - max_steady_state_speed = original_speed_limit + threshold_speed_limit_offset - # (seconds) Minimum required threshold at steady state after completing all geofence-triggered maneuvers - min_steady_state_time = 10.0 - - # Conduct steady state evaluation - # Get the start time of the vehicle reaching steady state (if one exists) - has_steady_state = False - for topic, msg, t in bag.read_messages(topics=['/hardware_interface/vehicle/twist'], start_time = time_end_engagement - ten_second_duration, end_time = time_end_engagement): - # Vehicle has reached steady state when its speed within threshold range of steady state speed - if (max_steady_state_speed >= msg.twist.linear.x >= min_steady_state_speed): - time_start_steady_state = t - has_steady_state = True - break - - time_steady_state = (time_end_engagement - time_start_steady_state).to_sec() - - if (time_steady_state >= 3.0): - print("WZ-23 succeeded; system ended use case at steady state for " + str(time_steady_state) + " seconds") - is_successful = True - else: - if has_steady_state: - print("WZ-23 failed; system ended use case at steady state for " + str(time_steady_state) + " seconds") - is_successful = False - if not has_steady_state: - print("WZ-23 failed; system did not reach steady state at end of use case.") - is_successful = False - - return is_successful - -########################################################################################################### -# Basic Travel WZ-24: The entire scenario will satisfy all previous criteria using any of the speeds given here for -# the "regular speed limit" (i.e. the speed limit when not in the geo-fence). -########################################################################################################### -def check_speed_limit_when_not_in_geofence(bag, original_speed_limit): - expected_speed_limit = 8.9408 # m/s (20 mph) - - # (m/s) Threshold offset from speed limit to account for floating point precision - threshold_speed_limit_offset = 0.01 - max_speed_limit = expected_speed_limit + threshold_speed_limit_offset # (m/s) - min_speed_limit = expected_speed_limit - threshold_speed_limit_offset # (m/s) - - is_successful = False - if min_speed_limit <= original_speed_limit <= max_speed_limit: - print("WZ-24 Succeeded; speed limit outside of geofence is " + str(original_speed_limit) + " m/s (20 mph)") - is_successful = True - else: - print("WZ-24 Failed; speed limit outside of geofence is " + str(original_speed_limit) + " m/s") - - return is_successful - -########################################################################################################### -# Workzone WZ-25: The advisory speed limit communicated by CARMA Cloud shall be 5 mph less than the -# regular speed limit. -########################################################################################################### -def check_advisory_speed_limit(bag, advisory_speed_limit, original_speed_limit): - # (m/s) Required offset from normal speed limit required for the advisory speed limit - speed_limit_offset = 2.2352 # 4.4704 m/s is 10 mph - # (m/s) Threshold offset from expected advisory speed limit to account for floating point precision - threshold_speed_limit_offset = 0.01 - max_advisory_speed_limit = (original_speed_limit - speed_limit_offset) + threshold_speed_limit_offset - min_advisory_speed_limit = (original_speed_limit - speed_limit_offset) - threshold_speed_limit_offset - - # Evaluate speed limits, print success/failure statement, and return success flag - advisory_speed_limit_mph = int(advisory_speed_limit * 2.23694) # Conversion of m/s to mph - original_speed_limit_mph = int(original_speed_limit * 2.23694) # Conversion of m/s to mph - speed_limit_offset_mph = int(speed_limit_offset * 2.23694) # Conversion of m/s to mph - if (max_advisory_speed_limit >= advisory_speed_limit >= min_advisory_speed_limit): - print("WZ-25 succeeded; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ - + " mph), which is " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ - + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") - is_successful = True - else: - print("WZ-25 failed; received advisory speed limit " + str(advisory_speed_limit) + " m/s (" + str(advisory_speed_limit_mph) \ - + " mph), which is not " + str(speed_limit_offset) + " m/s (" + str(speed_limit_offset_mph) + " mph) below the original speed limit " \ - + "of " + str(original_speed_limit) + " m/s (" + str(original_speed_limit_mph) + " mph)") - is_successful = False - - return is_successful - -# Main Function; run all tests from here -def main(): - if len(sys.argv) < 2: - print("Need 1 arguments: process_bag.py ") - exit() - - #source_folder = sys.argv[1] - - # Re-direct the output of print() to a specified .txt file: - #orig_stdout = sys.stdout - current_time = datetime.datetime.now() - #text_log_filename = "Results_" + str(current_time) + ".txt" - #text_log_file_writer = open(text_log_filename, 'w') - #sys.stdout = text_log_file_writer - - # Create .csv file to make it easier to view overview of results (the .txt log file is still used for more in-depth information): - #csv_results_filename = "Results_" + str(current_time) + ".csv" - #csv_results_writer = csv.writer(open(csv_results_filename, 'w')) - #csv_results_writer.writerow(["Bag Name", "Vehicle Name", "Test Type", - # "WZ-1 Result", "WZ-2 Result", "WZ-3 Result", "WZ-4 Result", "WZ-5 Result", "WZ-6 Result", - # "WZ-7 Result", "WZ-8 Result", "WZ-9 Result", "WZ-10 Result","WZ-11 Result", "WZ-12 Result", - # "WZ-13 Result", "WZ-14 Result", "WZ-15 Result", "WZ-16 Result", "WZ-17 Result", "WZ-18 Result", - # "WZ-19 Result", "WZ-20 Result", "WZ-21 Result", "WZ-22 Result", "WZ-23 Result", "WZ-24 Result", "WZ-25 Result"]) - - # Create list of Red Light Workzone Black Pacifica bag files to be processed - black_pacifica_red_bag_files = [] - - # Create list of Red Light Workzone Ford Fusion bag files to be processed - ford_fusion_red_bag_files = ["_2021-09-22-16-00-28-red.bag", - "_2021-09-22-19-03-26-red.bag"] - - # Create list of Red Light Workzone Blue Lexus bag files to be processed - blue_lexus_red_bag_files = [] - - # Create list of Green Light Workzone Black Pacifica bag files to be processed - black_pacifica_green_bag_files = [] - - # Create list of Green Light Workzone Ford Fusion bag files to be processed - ford_fusion_green_bag_files = [] - - # Create list of Green Light Workzone Blue Lexus bag files to be processed - blue_lexus_green_bag_files = [] - - # Concatenate all Basic Travel bag files into one list - red_light_bag_files = black_pacifica_red_bag_files + ford_fusion_red_bag_files + blue_lexus_red_bag_files - green_light_bag_files = black_pacifica_green_bag_files + ford_fusion_green_bag_files + blue_lexus_green_bag_files - WZ_bag_files = red_light_bag_files + green_light_bag_files - - # Loop to conduct data anlaysis on each bag file: - for bag_file in WZ_bag_files: - print("*****************************************************************") - print("Processing new bag: " + str(bag_file)) - if bag_file in black_pacifica_red_bag_files: - print("Black Pacifica Red Light Workzone Test Case") - elif bag_file in ford_fusion_red_bag_files: - print("Ford Fusion Red Light Workzone Test Case") - elif bag_file in blue_lexus_red_bag_files: - print("Blue Lexus Red Light Workzone Test Case") - if bag_file in black_pacifica_green_bag_files: - print("Black Pacifica Green Light Workzone Test Case") - elif bag_file in ford_fusion_green_bag_files: - print("Ford Fusion Green Light Workzone Test Case") - elif bag_file in blue_lexus_green_bag_files: - print("Blue Lexus Green Light Workzone Test Case") - - # Print processing progress to terminal (all other print statements are re-directed to outputted .txt file): - #sys.stdout = orig_stdout - print("Processing bag file " + str(bag_file) + " (" + str(WZ_bag_files.index(bag_file) + 1) + " of " + str(len(WZ_bag_files)) + ")") - #sys.stdout = text_log_file_writer - - # Process bag file if it exists and can be processed, otherwise skip and proceed to next bag file - try: - print("Starting To Process Bag at " + str(datetime.datetime.now())) - bag_file_path = str(source_folder) + "/" + bag_file - bag = rosbag.Bag(bag_file_path) - print("Finished Processing Bag at " + str(datetime.datetime.now())) - except: - print("Skipping " + str(bag_file) +", unable to open or process bag file.") - continue - - # Get the rosbag times associated with entering and exiting the active geofence - print("Getting geofence times at " + str(datetime.datetime.now())) - time_enter_geofence, time_exit_geofence, found_geofence_times = get_geofence_entrance_and_exit_times(bag) - print("Got geofence times at " + str(datetime.datetime.now())) - if (not found_geofence_times): - print("Could not find geofence entrance and exit times in bag file.") - continue - - # Get the rosbag times associated with the starting engagement and ending engagement for the Basic Travel use case test - print("Getting engagement times at " + str(datetime.datetime.now())) - time_test_start_engagement, time_test_end_engagement, found_test_times = get_test_case_engagement_times(bag, time_enter_geofence, time_exit_geofence) - print("Got engagement times at " + str(datetime.datetime.now())) - if (not found_test_times): - print("Could not find test case engagement start and end times in bag file.") - continue - - # Debug Statements - print("Engagement starts at " + str(time_test_start_engagement.to_sec())) - print("Entered Geofence at " + str(time_enter_geofence.to_sec())) - print("Exited Geofence at " + str(time_exit_geofence.to_sec())) - print("Engagement ends at " + str(time_test_end_engagement.to_sec())) - print("Time spent in geofence: " + str((time_exit_geofence - time_enter_geofence).to_sec()) + " seconds") - print("Time spent engaged: " + str((time_test_end_engagement - time_test_start_engagement).to_sec()) + " seconds") - - original_speed_limit = get_route_original_speed_limit(bag, time_test_start_engagement) # Units: m/s - print("Original Speed Limit is " + str(original_speed_limit) + " m/s") - - # Update the exit geofence time to be based off of /guidance/route_state for improved accuracy - time_exit_geofence = adjust_geofence_exit_time(bag, time_exit_geofence, original_speed_limit) - print("Adjusted geofence exit time (based on /guidance/route_state): " + str(time_exit_geofence)) - - print_lanelet_entrance_times(bag, time_test_start_engagement) - - downtrack_enter_geofence = get_geofence_entrance_downtrack(bag, time_enter_geofence) - - # Initialize results - wz_1_result = None - wz_2_result = None - wz_3_result = None - wz_4_result = None - wz_5_result = None - wz_6_result = None - wz_7_result = None - wz_8_result = None - wz_9_result = None - wz_10_result = None - wz_11_result = None - wz_12_result = None - wz_13_result = None - wz_14_result = None - wz_15_result = None - wz_16_result = None - wz_17_result = None - wz_18_result = None - wz_19_result = None - wz_20_result = None - wz_21_result = None - wz_22_result = None - wz_23_result = None - wz_24_result = None - wz_25_result = None - wz_26_result = None - wz_27_result = None - - # Metrics WZ-4, WZ-5, WZ-6, WZ-7, and WZ-8 - advisory_speed_limit, time_first_msg_received, wz_4_result, wz_5_result, wz_6_result, wz_7_result, wz_8_result = get_workzone_TCM_data(bag) - - # Convert advisory speed limit from WZ-19 to m/s for future metric evaluations - advisory_speed_limit = advisory_speed_limit * 0.44704 # Conversion from mph to m/s - - lanelets_in_geofence = get_geofence_lanelets(bag, time_enter_geofence, advisory_speed_limit) - - # Metric WZ-1 and WZ-5 - closed_lanelets = [12459, 1245999, 1245998] - wz_1_result, wz_5_result = check_geofence_in_initial_route(bag, closed_lanelets) - - # Metric WZ-2 - wz_2_result = check_steady_state_before_first_received_message(bag, time_test_start_engagement, time_first_msg_received, original_speed_limit) - - # Metric WZ-9 - wz_9_result = check_percentage_successful_spat_msg(bag, time_test_start_engagement, time_test_end_engagement) - - # Metric WZ-10 - wz_10_result = check_duration_between_spat_msg_below_max_duration(bag, time_test_start_engagement, time_test_end_engagement) - - stop_bar_location = [-36.8063, 323.251] # Map [x,y] coordinate of stop bar. Hardcoded; same value for every test - #end_geofence_location = [0,0] # Map [x,y] coordinate of the end of the geofence. Hardcoded; same value for every test - - if bag_file in red_light_bag_files: - wz_11_result, time_last_accel, time_stopped = check_deceleration_for_red_light(bag, time_test_start_engagement) - - dist_rear_axle_to_front_bumper = 4.0 # TODO: Obtain improved measurement for this distance for each vehicle - wz_12_result = check_stop_location_for_red_light(bag, stop_bar_location, dist_rear_axle_to_front_bumper, time_test_start_engagement, time_last_accel, time_stopped) - #generate_speed_plot(bag) - - wz_13_result = check_acceleration_time_after_green_light(bag, time_stopped) - - wz_14_result = check_acceleration_after_stop(bag, time_stopped) - - print("WZ-15 N/A (Red Light Bag)") - print("WZ-16 N/A (Red Light Bag)") - - else: - print("WZ-11 N/A (Green Light Bag)") - print("WZ-12 N/A (Green Light Bag)") - print("WZ-13 N/A (Green Light Bag)") - print("WZ-14 N/A (Green Light Bag)") - - - wz_15_result, time_vehicle_at_stop_bar = check_vehicle_speed_at_green_light(bag, advisory_speed_limit, stop_bar_location, time_test_start_engagement) - - wz_16_result = check_deceleration_after_green_light(bag, time_vehicle_at_stop_bar, time_test_start_engagement) - - wz_23_result = check_steady_state_after_geofence(bag, time_test_end_engagement, original_speed_limit) - - wz_24_result = check_speed_limit_when_not_in_geofence(bag, original_speed_limit) - - wz_25_result = check_advisory_speed_limit(bag, advisory_speed_limit, original_speed_limit) - - # Get vehicle type that this bag file is from - vehicle_name = "Unknown" - if bag_file in black_pacifica_green_bag_files or bag_file in black_pacifica_red_bag_files: - vehicle_name = "Black Pacifica" - elif bag_file in ford_fusion_green_bag_files or bag_file in ford_fusion_red_bag_files: - vehicle_name = "Ford Fusion" - else: - vehicle_name = "N/A" - - # Get test type that this bag file is for - vehicle_role = "Unknown" - if bag_file in red_light_bag_files: - vehicle_role = "Red Light" - elif bag_file in green_light_bag_files: - vehicle_role = "Green Light" - - # Write simple pass/fail results to .csv file for appropriate row: - #csv_results_writer.writerow([bag_file, vehicle_name, vehicle_role, - # wz_1_result, wz_2_result, wz_3_result, wz_4_result, wz_5_result, wz_6_result, wz_7_result, - # wz_8_result, wz_9_result, wz_10_result, wz_11_result, wz_12_result, wz_13_result, wz_14_result, - # wz_15_result, wz_16_result, wz_17_result, wz_18_result, wz_19_result, wz_20_result, - # wz_21_result, wz_22_result, wz_23_result, wz_24_result, wz_25_result]) - - #sys.stdout = orig_stdout - #text_log_file_writer.close() - return - -if __name__ == "__main__": - main() \ No newline at end of file From 1598c5f4e341ca79c20b4ff5791f131292cb46c0 Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Mon, 8 Aug 2022 10:06:14 -0400 Subject: [PATCH 059/165] Update sonar-scanner.properties --- .sonarqube/sonar-scanner.properties | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index dd3f8e862b..4cf717811d 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -22,8 +22,7 @@ sonar.host.url=https://sonarcloud.io sonar.sources=src/main sonar.tests=src/test sonar.cfamily.gcov.reportsPath=/opt/carma/coverage_reports/gcov -sonar.coverageReportPaths=/opt/carma/coverage_reports/gcov/coverage01.html -sonar.cfamily.vscoveragexml.reportsPath=/opt/carma/coverage_reports/gcov/coverage.xml +sonar.coverageReportPaths=/opt/carma/coverage_reports/gcov/coverage.xml # Set Git as SCM sensor sonar.scm.disabled=false sonar.scm.enabled=true From fa93cb86d8ee9d3bac66db1e440dcb6483119812 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Mon, 8 Aug 2022 16:56:58 -0700 Subject: [PATCH 060/165] Fix build error --- lci_strategic_plugin/src/lci_strategic_plugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index 9b05dee147..ef394d766c 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -919,9 +919,9 @@ cav_msgs::MobilityOperation LCIStrategicPlugin::generateMobilityOperation() if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right"; if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left"; - mo_.strategy_params = "access: " + "1" + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets + mo_.strategy_params = "access: 1" + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) + - ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: " + "0" + // NOTE: Departure position set to 0 at all times since it's not specified by streets + ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: 0" + // NOTE: Departure position set to 0 at all times since it's not specified by streets ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_); From 0bf2a7344ab03992224ff08040ea08a63618b85b Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Tue, 9 Aug 2022 09:39:26 -0700 Subject: [PATCH 061/165] fix build error --- lci_strategic_plugin/src/lci_strategic_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index ef394d766c..8eff872294 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -919,7 +919,7 @@ cav_msgs::MobilityOperation LCIStrategicPlugin::generateMobilityOperation() if (intersection_turn_direction_ == TurnDirection::Right) intersection_turn_direction = "right"; if (intersection_turn_direction_ == TurnDirection::Left) intersection_turn_direction = "left"; - mo_.strategy_params = "access: 1" + ", max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets + mo_.strategy_params = "access: 1, max_accel: " + std::to_string(vehicle_acceleration_limit_) + // NOTE: Access currently set to 1 at all times since its not specified by streets ", max_decel: " + std::to_string(vehicle_deceleration_limit_) + ", react_time: " + std::to_string(config_.reaction_time) + ", min_gap: " + std::to_string(config_.min_gap) + ", depart_pos: 0" + // NOTE: Departure position set to 0 at all times since it's not specified by streets ", turn_direction: " + intersection_turn_direction + ", msg_count: " + std::to_string(bsm_msg_count_) + ", sec_mark: " + std::to_string(bsm_sec_mark_); From 80fd631f92ba4a62ba7752545ad5ee05acea1fac Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Thu, 11 Aug 2022 10:13:03 -0700 Subject: [PATCH 062/165] WIP port lci tactical plugin to ROS2 --- .../CMakeLists.txt | 103 ++-- .../README.md | 3 + .../config/parameters.yaml | 0 .../light_controlled_intersection_config.h | 83 --- .../light_controlled_intersection_plugin.h | 220 -------- ...ight_controlled_intersection_plugin_node.h | 118 ----- ...ed_intersection_tactical_plugin_config.hpp | 78 +++ ...lled_intersection_tactical_plugin_node.hpp | 221 ++++++++ ...rolled_intersection_tactical_plugin.launch | 20 - ...led_intersection_tactical_plugin_launch.py | 68 +++ .../package.xml | 45 +- ...ontrolled_intersection_tactical_plugin.cpp | 429 --------------- ...lled_intersection_tactical_plugin_node.cpp | 496 ++++++++++++++++++ .../src/main.cpp | 22 +- .../test/node_test.cpp | 49 ++ .../test/test_lci_tactical_plugin.cpp | 116 ---- 16 files changed, 992 insertions(+), 1079 deletions(-) mode change 100755 => 100644 light_controlled_intersection_tactical_plugin/CMakeLists.txt create mode 100644 light_controlled_intersection_tactical_plugin/README.md mode change 100755 => 100644 light_controlled_intersection_tactical_plugin/config/parameters.yaml delete mode 100755 light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_config.h delete mode 100755 light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin.h delete mode 100755 light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h create mode 100644 light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp create mode 100644 light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp delete mode 100755 light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin.launch create mode 100644 light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py mode change 100755 => 100644 light_controlled_intersection_tactical_plugin/package.xml delete mode 100755 light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp create mode 100644 light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp mode change 100755 => 100644 light_controlled_intersection_tactical_plugin/src/main.cpp create mode 100644 light_controlled_intersection_tactical_plugin/test/node_test.cpp delete mode 100755 light_controlled_intersection_tactical_plugin/test/test_lci_tactical_plugin.cpp diff --git a/light_controlled_intersection_tactical_plugin/CMakeLists.txt b/light_controlled_intersection_tactical_plugin/CMakeLists.txt old mode 100755 new mode 100644 index c66358ba9c..cc98c8e754 --- a/light_controlled_intersection_tactical_plugin/CMakeLists.txt +++ b/light_controlled_intersection_tactical_plugin/CMakeLists.txt @@ -1,4 +1,4 @@ -# + # Copyright (C) 2022 LEIDOS. # # Licensed under the Apache License, Version 2.0 (the "License"); you may not @@ -12,92 +12,61 @@ # 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(light_controlled_intersection_tactical_plugin) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -set(CATKIN_DEPS - roscpp - cav_msgs - cav_srvs - carma_utils - trajectory_utils - carma_wm - basic_autonomy - lanelet2_core -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED) -find_package(Eigen3 REQUIRED) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -################################### -## catkin specific configuration ## -################################### +# Name build targets +set(node_exec light_controlled_intersection_tactical_plugin_node_exec) +set(node_lib light_controlled_intersection_tactical_plugin_node) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost EIGEN3 +# Includes +include_directories( + include ) -########### -## Build ## -########### +# Build +ament_auto_add_library(${node_lib} SHARED + src/light_controlled_intersection_tactical_plugin_node.cpp +) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} +ament_auto_add_executable(${node_exec} + src/main.cpp ) -add_executable( ${PROJECT_NAME} - src/main.cpp) +# Register component +rclcpp_components_register_nodes(${node_lib} "light_controlled_intersection_tactical_plugin::Node") -add_library(light_controlled_intersection_tactical_library - src/light_controlled_intersection_tactical_plugin.cpp) +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} +) -target_link_libraries(light_controlled_intersection_tactical_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# Testing +if(BUILD_TESTING) -add_dependencies(light_controlled_intersection_tactical_library ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -target_link_libraries(${PROJECT_NAME} light_controlled_intersection_tactical_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + ament_add_gtest(test_light_controlled_intersection_tactical_plugin test/node_test.cpp) -############# -## Install ## -############# + ament_target_dependencies(test_light_controlled_intersection_tactical_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) + target_link_libraries(test_light_controlled_intersection_tactical_plugin ${node_lib}) -## Install C++ -install(TARGETS ${PROJECT_NAME} light_controlled_intersection_tactical_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +endif() -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch ) - -############# -## Testing ## -############# -# catkin_add_gmock(${PROJECT_NAME}-test -# test/test_lci_tactical_plugin.cpp -# WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) -# target_link_libraries(${PROJECT_NAME}-test light_controlled_intersection_tactical_library ${catkin_LIBRARIES}) \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/README.md b/light_controlled_intersection_tactical_plugin/README.md new file mode 100644 index 0000000000..f194feb521 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/README.md @@ -0,0 +1,3 @@ +# light_controlled_intersection_tactical_plugin + +TODO for USER: Add description of package and link to confluence documentation. diff --git a/light_controlled_intersection_tactical_plugin/config/parameters.yaml b/light_controlled_intersection_tactical_plugin/config/parameters.yaml old mode 100755 new mode 100644 diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_config.h b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_config.h deleted file mode 100755 index 9abd737338..0000000000 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_config.h +++ /dev/null @@ -1,83 +0,0 @@ -#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. - */ - -#include - -/** - * \brief Stuct containing the algorithm configuration values for the LightControlledIntersectionTacticalPlugin - */ -struct LightControlledIntersectionTacticalPluginConfig -{ - int default_downsample_ratio = 36; // Amount to downsample input lanelet centerline data. Value corresponds to saving each nth point. - int turn_downsample_ratio = 20; // Amount to downsample input lanelet centerline data on turns. Value corresponds to saving each nth point. - double trajectory_time_length = 6.0; // Trajectory length in seconds - double curve_resample_step_size = 1.0; // Curve re-sampling step size in m - int curvature_moving_average_window_size = 9; // Size of the window used in the moving average filter to smooth the curvature profile - // computed curvature and output speeds - double lateral_accel_limit = 2.5; // Maximum allowable lateral acceleration m/s^2 - double lat_accel_multiplier = 0.50; // Multiplier of lat_accel to bring the value under lat_accel - int speed_moving_average_window_size = 5; // Size of the window used in the moving average filter to smooth both the speed profile - double back_distance = 20; // Number of meters behind the first maneuver that need to be included in points for curvature calculation - double buffer_ending_downtrack = 20.0; // Additional distance beyond ending downtrack to ensure sufficient points - //! The maximum allowable vehicle deceleration limit in m/s - double vehicle_decel_limit = 2.0; - double minimum_speed = 2.2352; // minimum allowable speed in m/s - - //! A multiplier to apply to the maximum allowable vehicle deceleration limit so we plan under our capabilities - double vehicle_decel_limit_multiplier = 0.75; - - //! The maximum allowable vehicle acceleration limit in m/s - double vehicle_accel_limit = 2.0; - - //! A multiplier to apply to the maximum allowable vehicle acceleration limit so we plan under our capabilities - double vehicle_accel_limit_multiplier = 0.75; - - //! A buffer infront of the stopping location which will still be considered a valid stop - double stop_line_buffer = 2.0; - - //! Distance from the nearest traffic light where the vehicle decides whether to run last successful trajectory or accept new one (in meters) - double algorithm_evaluation_distance = 35.0; - - //! Period if scheduled entry time is within which the vehicle decides to run the last successful trajectory smoothing trajectory (in seconds) - double algorithm_evaluation_period = 4.5; - - friend std::ostream& operator<<(std::ostream& output, const LightControlledIntersectionTacticalPluginConfig& c) - { - output <<"LightControlledIntersectionTacticalPluginConfig { " < -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -/** - * \brief Macro definition to enable easier access to fields shared across the maneuver types - * \param mvr The maneuver object to invoke the accessors on - * \param property The name of the field to access on the specific maneuver types. Must be shared by all extant maneuver types - * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field - */ -#define GET_MANEUVER_PROPERTY(mvr, property)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ - throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))) - -namespace light_controlled_intersection_transit_plugin -{ -using PublishPluginDiscoveryCB = std::function; -using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; -using GeneralTrajConfig = basic_autonomy::waypoint_generation::GeneralTrajConfig; -using DetailedTrajConfig = basic_autonomy::waypoint_generation::DetailedTrajConfig; - -enum TSCase { - CASE_1 = 1, - CASE_2 = 2, - CASE_3 = 3, - CASE_4 = 4, - CASE_5 = 5, - CASE_6 = 6, - CASE_7 = 7, - CASE_8 = 8, -}; - -struct TrajectoryParams -{ - double a1_ = 0; - double v1_ = 0; - double x1_ = 0; - - double a2_ = 0; - double v2_ = 0; - double x2_ = 0; - - double a3_ = 0; - double v3_ = 0; - double x3_ = 0; -}; - -/** - * \brief Class containing primary business logic for the Light Controlled Intersection Tactical Plugin - * - */ - -class LightControlledIntersectionTacticalPlugin -{ -public: - /** - * \brief Constructor - * - * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data - * \param config The configuration to be used for this object - * \param plugin_discovery_publisher Callback which will publish the current plugin discovery state - * \param debug_publisher Callback which will publish a debug message. The callback defaults to no-op. - */ - LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm,const LightControlledIntersectionTacticalPluginConfig& config, - const PublishPluginDiscoveryCB &plugin_discovery_publisher); - - /** - * \brief Service callback for trajectory planning - * - * \param req The service request - * \param resp The service response - * - * \return True if success. False otherwise - */ - bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp); - - /** - * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. - * \param wm world_model pointer - * \param points_and_target_speeds of centerline points paired with speed limits whose speeds are to be modified: - * \param start_dist starting downtrack of the maneuver to be planned (excluding buffer points) in m - * \param remaining_dist distance for the maneuver to be planned (excluding buffer points) in m - * \param starting_speed starting speed at the start of the maneuver in m/s - * \param departure_speed ending speed of the maneuver a.k.a entry speed into the intersection m/s - * \param tsp trajectory smoothing parameters - - * NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. - * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_ - */ - void apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, - double starting_speed, double departure_speed, TrajectoryParams tsp); - - /** - * \brief Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals. - * Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing - * \param maneuver Maneuver associated that has starting downtrack and desired entry time - * \param starting_speed Starting speed of the vehicle - * \param points The set of points with raw speed limits whose speed profile to be changed. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. - * - */ - void apply_optimized_target_speed_profile(const cav_msgs::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); - - /** - * \brief Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) - * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated raw speed limits - * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. - * If the first maneuver exceeds this then it's downtrack will be shifted to this value. - * \param wm Pointer to intialized world model for semantic map access - * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later - * \param state The vehicle state at the time the function is called - * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins - * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver - * NOTE: This function is a slightly modified version of the same function in basic_autonomy library and currently only plans for the first maneuver - */ - std::vector create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, - cav_msgs::VehicleState &ending_state_before_buffer,const cav_msgs::VehicleState& state, - const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); - - /** - * \brief Given a Lanelet, find it's associated Speed Limit - * - * \param llt Constant Lanelet object - * - * \throw std::invalid_argument if the speed limit could not be retrieved - * - * \return value of speed limit in mps - */ - double findSpeedLimit(const lanelet::ConstLanelet& llt) const; - - /** - * \brief Method to call at fixed rate in execution loop. Will publish plugin discovery updates - */ - - void onSpin(); - - ////////// VARIABLES /////////// - // CARMA Streets Variables - // timestamp for msg received from carma streets - uint32_t street_msg_timestamp_ = 0.0; - // scheduled stop time - uint32_t scheduled_stop_time_ = 0.0; - // scheduled enter time - uint32_t scheduled_enter_time_ = 0.0; - // scheduled depart time - uint32_t scheduled_depart_time_ = 0.0; - // scheduled latest depart time - uint32_t scheduled_latest_depart_time_ = 0.0; - // flag to show if the vehicle is allowed in intersection - bool is_allowed_int_ = false; - - // approximate speed limit - double speed_limit_ = 11.176; //25mph by default - boost::optional last_case_; - boost::optional is_last_case_successful_; - cav_msgs::TrajectoryPlan last_trajectory_; - - cav_msgs::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later - - // downtrack of host vehicle - double current_downtrack_ = 0.0; - - double last_successful_ending_downtrack_; // if algorithm was successful, this is traffic_light_downtrack - double last_successful_scheduled_entry_time_; // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) - - private: - - carma_wm::WorldModelConstPtr wm_; - LightControlledIntersectionTacticalPluginConfig config_; - PublishPluginDiscoveryCB plugin_discovery_publisher_; - - cav_msgs::Plugin plugin_discovery_msg_; - carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_; - std::vector last_final_speeds_; - - std::string light_controlled_intersection_strategy_ = "signalized"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends - - double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with -}; -}; \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h deleted file mode 100755 index c765cf9133..0000000000 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_plugin_node.h +++ /dev/null @@ -1,118 +0,0 @@ -#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. - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace light_controlled_intersection_transit_plugin -{ - /** - * \brief ROS node for the LightControlledIntersectionTransitPlugin - */ -class LightControlledIntersectionTransitPluginNode -{ -public: - /** - * \brief Entrypoint for this node - */ - void run() - { - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - carma_wm::WMListener wml; - auto wm_ = wml.getWorldModel(); - - LightControlledIntersectionTacticalPluginConfig config; - - pnh.param("trajectory_time_length", config.trajectory_time_length, config.trajectory_time_length); - pnh.param("curve_resample_step_size", config.curve_resample_step_size, config.curve_resample_step_size); - pnh.param("default_downsample_ratio", config.default_downsample_ratio, config.default_downsample_ratio); - pnh.param("turn_downsample_ratio", config.turn_downsample_ratio, config.turn_downsample_ratio); - pnh.param("vehicle_decel_limit_multiplier", config.vehicle_decel_limit_multiplier, config.vehicle_decel_limit_multiplier); - pnh.param("vehicle_accel_limit_multiplier", config.vehicle_accel_limit_multiplier, config.vehicle_accel_limit_multiplier); - pnh.param("lat_accel_multiplier", config.lat_accel_multiplier, config.lat_accel_multiplier); - pnh.param("back_distance", config.back_distance, config.back_distance); - pnh.param("speed_moving_average_window_size", config.speed_moving_average_window_size, - config.speed_moving_average_window_size); - pnh.param("curvature_moving_average_window_size", config.curvature_moving_average_window_size, - config.curvature_moving_average_window_size); - pnh.param("buffer_ending_downtrack", config.buffer_ending_downtrack, config.buffer_ending_downtrack); - pnh.param("/vehicle_deceleration_limit", config.vehicle_decel_limit, config.vehicle_decel_limit); - pnh.param("/vehicle_acceleration_limit", config.vehicle_accel_limit, config.vehicle_accel_limit); - pnh.param("/vehicle_lateral_accel_limit", config.lateral_accel_limit, config.lateral_accel_limit); - pnh.param("stop_line_buffer", config.stop_line_buffer, config.stop_line_buffer); - pnh.param("algorithm_evaluation_distance", config.algorithm_evaluation_distance, config.algorithm_evaluation_distance); - pnh.param("algorithm_evaluation_period", config.algorithm_evaluation_period, config.algorithm_evaluation_period); - pnh.param("minimum_speed", config.minimum_speed, config.minimum_speed); - - ROS_INFO_STREAM("LightControlledIntersectionTacticalPlugin Params" << config); - - config.lateral_accel_limit = config.lateral_accel_limit * config.lat_accel_multiplier; - config.vehicle_accel_limit = config.vehicle_accel_limit * config.vehicle_accel_limit_multiplier; - config.vehicle_decel_limit = config.vehicle_decel_limit * config.vehicle_decel_limit_multiplier; - - ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); - - LightControlledIntersectionTacticalPlugin worker(wm_, config, [&discovery_pub](auto& msg) { discovery_pub.publish(msg); }); - - ros::ServiceServer trajectory_srv_ = nh.advertiseService("light_controlled_intersection_tactical_plugin/plan_trajectory", - &LightControlledIntersectionTacticalPlugin::plan_trajectory_cb, &worker); - - ros::Timer discovery_pub_timer_ = - pnh.createTimer(ros::Duration(ros::Rate(10.0)), [&worker](const auto&) { worker.onSpin(); }); - - ros::CARMANodeHandle::spin(); - } -}; - -} //namespace light_controlled_intersection_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp new file mode 100644 index 0000000000..c2e333c44d --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp @@ -0,0 +1,78 @@ +#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. + */ + +#include +#include + +namespace light_controlled_intersection_tactical_plugin +{ + + /** + * \brief Stuct containing the algorithm configuration values for light_controlled_intersection_tactical_plugin + */ + struct Config + { + double centerline_sampling_spacing; + double trajectory_time_length; + int default_downsample_ratio; + int turn_downsample_ratio; + double curve_resample_step_size; + int curvature_moving_average_window_size; + int speed_moving_average_window_size; + double back_distance; + double buffer_ending_downtrack; + double vehicle_decel_limit_multiplier; + double vehicle_accel_limit_multiplier; + double lat_accel_multiplier; + double stop_line_buffer; + double minimum_speed; + double algorithm_evaluation_distance; + double algorithm_evaluation_period; + double vehicle_lateral_accel_limit; + double vehicle_acceleration_limit; + double vehicle_deceleration_limit; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "light_controlled_intersection_tactical_plugin::Config { " << std::endl + << "centerline_sampling_spacing: " << c.centerline_sampling_spacing << std::endl + << "trajectory_time_length: " << c.trajectory_time_length << std::endl + << "default_downsample_ratio: " << c.default_downsample_ratio << std::endl + << "turn_downsample_ratio: " << c.turn_downsample_ratio << std::endl + << "curve_resample_step_size: " << c.curve_resample_step_size << std::endl + << "curvature_moving_average_window_size: " << c.curvature_moving_average_window_size << std::endl + << "speed_moving_average_window_size: " << c.speed_moving_average_window_size << std::endl + << "back_distance: " << c.back_distance << std::endl + << "buffer_ending_downtrack: " << c.buffer_ending_downtrack << std::endl + << "vehicle_decel_limit_multiplier: " << c.vehicle_decel_limit_multiplier << std::endl + << "vehicle_accel_limit_multiplier: " << c.vehicle_accel_limit_multiplier << std::endl + << "lat_accel_multiplier: " << c.lat_accel_multiplier << std::endl + << "stop_line_buffer: " << c.stop_line_buffer << std::endl + << "minimum_speed: " << c.minimum_speed << std::endl + << "algorithm_evaluation_distance: " << c.algorithm_evaluation_distance << std::endl + << "algorithm_evaluation_period: " << c.algorithm_evaluation_period << std::endl + << "vehicle_lateral_accel_limit: " << c.vehicle_lateral_accel_limit << std::endl + << "vehicle_acceleration_limit: " << c.vehicle_acceleration_limit << std::endl + << "vehicle_deceleration_limit: " << c.vehicle_deceleration_limit << std::endl + << "}" << std::endl; + return output; + } + }; + +} // light_controlled_intersection_tactical_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp new file mode 100644 index 0000000000..c495bcb3ac --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp @@ -0,0 +1,221 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp" + +/** + * \brief Macro definition to enable easier access to fields shared across the maneuver types + * \param mvr The maneuver object to invoke the accessors on + * \param property The name of the field to access on the specific maneuver types. Must be shared by all extant maneuver types + * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field + */ +#define GET_MANEUVER_PROPERTY(mvr, property)\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ + throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))) + +namespace light_controlled_intersection_tactical_plugin +{ + using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; + using GeneralTrajConfig = basic_autonomy::waypoint_generation::GeneralTrajConfig; + using DetailedTrajConfig = basic_autonomy::waypoint_generation::DetailedTrajConfig; + + enum TSCase { + CASE_1 = 1, + CASE_2 = 2, + CASE_3 = 3, + CASE_4 = 4, + CASE_5 = 5, + CASE_6 = 6, + CASE_7 = 7, + CASE_8 = 8, + }; + + struct TrajectoryParams { + double a1_ = 0; + double v1_ = 0; + double x1_ = 0; + + double a2_ = 0; + double v2_ = 0; + double x2_ = 0; + + double a3_ = 0; + double v3_ = 0; + double x3_ = 0; + }; + + /** + * \brief ROS node for the LightControlledIntersectionTransitPluginNode + * + */ + class LightControlledIntersectionTransitPluginNode : public carma_guidance_plugins::TacticalPlugin + { + + private: + // LightControlledIntersectionTransitPluginNode configuration + Config config_; + + // CARMA Streets Variables + // timestamp for msg received from carma streets + uint32_t street_msg_timestamp_ = 0.0; + // scheduled stop time + uint32_t scheduled_stop_time_ = 0.0; + // scheduled enter time + uint32_t scheduled_enter_time_ = 0.0; + // scheduled depart time + uint32_t scheduled_depart_time_ = 0.0; + // scheduled latest depart time + uint32_t scheduled_latest_depart_time_ = 0.0; + // flag to show if the vehicle is allowed in intersection + bool is_allowed_int_ = false; + + double speed_limit_ = 11.176; // Approximate speed limit; 25 mph by default + boost::optional last_case_; + boost::optional is_last_case_successful_; + carma_planning_msgs::msg::TrajectoryPlan last_trajectory_; + + carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later + + double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with + + // downtrack of host vehicle + double current_downtrack_ = 0.0; + + double last_successful_ending_downtrack_; // if algorithm was successful, this is traffic_light_downtrack + double last_successful_scheduled_entry_time_; // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) + + carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_; + std::vector last_final_speeds_; + + std::string light_controlled_intersection_strategy_ = "signalized"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends + + public: + /** + * \brief LightControlledIntersectionTransitPluginNode constructor + */ + explicit LightControlledIntersectionTransitPluginNode(const rclcpp::NodeOptions &); + + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. + * \param wm world_model pointer + * \param points_and_target_speeds of centerline points paired with speed limits whose speeds are to be modified: + * \param start_dist starting downtrack of the maneuver to be planned (excluding buffer points) in m + * \param remaining_dist distance for the maneuver to be planned (excluding buffer points) in m + * \param starting_speed starting speed at the start of the maneuver in m/s + * \param departure_speed ending speed of the maneuver a.k.a entry speed into the intersection m/s + * \param tsp trajectory smoothing parameters + * NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. + * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_ + */ + void apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + double starting_speed, double departure_speed, TrajectoryParams tsp); + + /** + * \brief Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals. + * Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing + * \param maneuver Maneuver associated that has starting downtrack and desired entry time + * \param starting_speed Starting speed of the vehicle + * \param points The set of points with raw speed limits whose speed profile to be changed. + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * + */ + void apply_optimized_target_speed_profile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); + + /** + * \brief Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) + * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated raw speed limits + * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. + * If the first maneuver exceeds this then it's downtrack will be shifted to this value. + * \param wm Pointer to intialized world model for semantic map access + * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later + * \param state The vehicle state at the time the function is called + * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins + * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins + * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver + * NOTE: This function is a slightly modified version of the same function in basic_autonomy library and currently only plans for the first maneuver + */ + std::vector create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, + carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, + const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); + + /** + * \brief Given a Lanelet, find it's associated Speed Limit + * + * \param llt Constant Lanelet object + * + * \throw std::invalid_argument if the speed limit could not be retrieved + * + * \return value of speed limit in mps + */ + double findSpeedLimit(const lanelet::ConstLanelet& llt) const; + + //// + // Overrides + //// + void plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; + + bool get_availability() override; + + std::string get_version_id() override; + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + }; + +} // light_controlled_intersection_tactical_plugin diff --git a/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin.launch b/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin.launch deleted file mode 100755 index 9a32aa2911..0000000000 --- a/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - diff --git a/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py b/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py new file mode 100644 index 0000000000..4d2812a336 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA light_controlled_intersection_tactical_plugin_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='light_controlled_intersection_tactical_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='light_controlled_intersection_tactical_plugin', + plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', + name='light_controlled_intersection_tactical_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/light_controlled_intersection_tactical_plugin/package.xml b/light_controlled_intersection_tactical_plugin/package.xml old mode 100755 new mode 100644 index 962394498b..24a1557dda --- a/light_controlled_intersection_tactical_plugin/package.xml +++ b/light_controlled_intersection_tactical_plugin/package.xml @@ -2,13 +2,10 @@ - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index c9e5e631bf..dd4cafac64 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -78,6 +78,9 @@ def generate_launch_description(): inlanecruising_plugin_file_path = os.path.join( get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml') + + trajectory_visualizer_param_file = os.path.join( + get_package_share_directory('trajectory_visualizer'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 3cc4a604bf..10e23cb0d5 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -51,7 +51,7 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/stop_and_wait_plugin_params.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') diff --git a/stop_and_wait_plugin/include/stop_and_wait_node.hpp b/stop_and_wait_plugin/include/stop_and_wait_node.hpp index 8dc0803132..5125760b5b 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_node.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_node.hpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp index 3baa7e30c7..9f44627c4a 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include using oss = std::ostringstream; diff --git a/trajectory_visualizer/config/params.yaml b/trajectory_visualizer/config/parameters.yaml similarity index 100% rename from trajectory_visualizer/config/params.yaml rename to trajectory_visualizer/config/parameters.yaml diff --git a/trajectory_visualizer/launch/trajectory_visualizer_launch.py b/trajectory_visualizer/launch/trajectory_visualizer_launch.py index e081be91c1..a8685aa91d 100644 --- a/trajectory_visualizer/launch/trajectory_visualizer_launch.py +++ b/trajectory_visualizer/launch/trajectory_visualizer_launch.py @@ -34,6 +34,10 @@ def generate_launch_description(): log_level = LaunchConfiguration('log_level') declare_log_level_arg = DeclareLaunchArgument( name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('trajectory_visualizer'), 'config/parameters.yaml') # Launch node(s) in a carma container to allow logging to be configured container = ComposableNodeContainer( @@ -52,6 +56,7 @@ def generate_launch_description(): {'use_intra_process_comms': True}, {'--log-level' : log_level } ], + parameters=[ param_file_path ] ), ] ) From 6c2eb54c06313c44b5d03081487b83fe568db367 Mon Sep 17 00:00:00 2001 From: Aswath <32398753+aswath1@users.noreply.github.com> Date: Mon, 15 Aug 2022 19:38:31 +0530 Subject: [PATCH 066/165] ui (#1881) This node is launched here carma-platform/carma/launch/ui.launch Line 57 in 350e30a and should be replaced with the equivalent ros2 node. There is a ROS2 version of rosbridge_suite https://github.com/RobotWebTools/rosbridge_suite This is the same framework used in CARMA3. --- carma/launch/carma_src.launch | 5 -- carma/launch/carma_src.launch.py | 25 +++++++- carma/launch/ui.launch | 61 ------------------ carma/launch/ui.launch.py | 106 +++++++++++++++++++++++++++++++ docker/checkout.bash | 5 ++ 5 files changed, 135 insertions(+), 67 deletions(-) delete mode 100644 carma/launch/ui.launch create mode 100644 carma/launch/ui.launch.py diff --git a/carma/launch/carma_src.launch b/carma/launch/carma_src.launch index 8b5f0b2fb9..a6dfd6a3ab 100755 --- a/carma/launch/carma_src.launch +++ b/carma/launch/carma_src.launch @@ -137,11 +137,6 @@ - - - - - diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py index a0a7ca5a0d..5341ea9b24 100644 --- a/carma/launch/carma_src.launch.py +++ b/carma/launch/carma_src.launch.py @@ -107,6 +107,14 @@ def generate_launch_description(): default_value= '[]', description='List of String: Guidance Control Plugins that will be validated by the Guidance Plugin Validator Node if enabled' ) + + # Declare port + port = LaunchConfiguration('port') + declare_port = DeclareLaunchArgument( + name = 'port', + default_value= "9090", + description='The default port for rosbridge is 909' + ) # Nodes @@ -201,6 +209,19 @@ def generate_launch_description(): arguments=['--ros-args', '--log-level', GetLogLevel('system_controller', env_log_levels)] ) + ui_group = GroupAction( + actions=[ + PushRosNamespace(EnvironmentVariable('CARMA_UI_NS', default_value='ui')), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/ui.launch.py']), + launch_arguments={ + 'port' : port + }.items() + ), + ] + ) + return LaunchDescription([ declare_vehicle_calibration_dir_arg, declare_vehicle_config_dir_arg, @@ -211,11 +232,13 @@ def generate_launch_description(): declare_strategic_plugins_to_validate, declare_tactical_plugins_to_validate, declare_control_plugins_to_validate, + declare_port, drivers_group, transform_group, environment_group, localization_group, v2x_group, guidance_group, - system_controller + system_controller, + ui_group ]) diff --git a/carma/launch/ui.launch b/carma/launch/ui.launch deleted file mode 100644 index e0f773ff4a..0000000000 --- a/carma/launch/ui.launch +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/carma/launch/ui.launch.py b/carma/launch/ui.launch.py new file mode 100644 index 0000000000..71fe357bfb --- /dev/null +++ b/carma/launch/ui.launch.py @@ -0,0 +1,106 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch.actions import Shutdown +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.substitutions import EnvironmentVariable +from carma_ros2_utils.launch.get_log_level import GetLogLevel +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +import os + + +def generate_launch_description(): + """ + UI/Ros_bridge nodes. + """ + env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') + + port = LaunchConfiguration('port') + declare_port = DeclareLaunchArgument( + name = 'port', + default_value = "9090", + description = "The default port for rosbridge is 9090" + ) + + carma_param_file = os.path.join( + get_package_share_directory('carma'), 'ui/config/CommandAPIParams.yaml') + + ui_container = ComposableNodeContainer( + package='carma_ros2_utils', # rclcpp_components + name='ui_container', + executable='lifecycle_component_wrapper_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='rosbridge_server', + plugin='rosbridge_websocket', + name='rosbridge_server', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('rosbridge_server', env_log_levels) }, + {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper + ], + remappings=[ + ("get_available_routes",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''),"/get_available_routes"]), + ("set_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/set_active_route"]), + ("start_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/start_active_route"]), + ("get_available_routes", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_available_routes"]), + ("route_state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_state]"), + ("route_event", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_event]"), + ("route", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route"]), + ("get_system_version", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_system_version"]), + ("state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state"]), + ("ui_platoon_vehicle_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ui_platoon_vehicle_info"]), + ("plugins/available_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/available_plugins"]), + ("plugins/get_registered_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/get_registered_plugins"]), + ("plugins/activate_plugin", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/activate_plugin"]), + ("get_available_routes", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_available_routes"]), + ("set_guidance_active", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/set_guidance_active"]), + ("plugins/controlling_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/controlling_plugins"]), + ("traffic_signal_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/traffic_signal_info"]), + ("platooning_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/platooning_info"]), + ("traffic_signal_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/traffic_signal_info"]), + ("system_alert", "/system_alert"), + + ("bsm", [EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm"]), + + ("nav_sat_fix", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/gnss/nav_sat_fix"]), + ("velocity", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist"]), + ("driver_discovery", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/driver_discovery"]), + ("get_drivers_with_capabilities", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/get_drivers_with_capabilities"]), + ("controller/robotic_status", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/robot_status"]), + ("controller/vehicle_cmd" , [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/vehicle_cmd"]), + ("comms/outbound_binary_msg",[EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/comms/outbound_binary_msg"]), + ("comms/inbound_binary_msg", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/comms/inbound_binary_msg"]), + ("can/engine_speed", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/engine_speed"]), + ("can/speed", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/speed"]), + ("can/acc_engaged", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/acc_engaged"]) + ], + parameters=[ carma_param_file + ] + ) + ]) + + return LaunchDescription([ + declare_port, + ui_container + ]) + + diff --git a/docker/checkout.bash b/docker/checkout.bash index ef0ace9e87..c7e4fc4e8a 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -63,3 +63,8 @@ cd ibeo_msgs echo "" > COLCON_IGNORE cd ../astuff_sensor_msgs echo "" > COLCON_IGNORE + +cd ../ + +#rosbridge_suite is a ROS meta-package including all the rosbridge packages. +git clone https://github.com/usdot-fhwa-stol/rosbridge_suite From e927332aee9417b70e7bc597cf21f1ea9a121cee Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Tue, 16 Aug 2022 12:17:23 -0700 Subject: [PATCH 067/165] Updates for lci tactical plugin ROS2 --- carma/launch/guidance.launch | 3 - carma/launch/guidance.launch.py | 22 +- .../CMakeLists.txt | 9 + ...ontrolled_intersection_tactical_plugin.hpp | 218 ++++++++++ ...lled_intersection_tactical_plugin_node.hpp | 141 +------ ...ontrolled_intersection_tactical_plugin.cpp | 389 ++++++++++++++++++ ...lled_intersection_tactical_plugin_node.cpp | 389 +----------------- .../test/node_test.cpp | 19 +- 8 files changed, 668 insertions(+), 522 deletions(-) create mode 100644 light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp create mode 100644 light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 14abc2e0a8..01f0907966 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -193,9 +193,6 @@ - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index c9e5e631bf..a186a464a5 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -244,8 +244,26 @@ def generate_launch_description(): parameters=[ trajectory_visualizer_param_file ] - ) - + ), + ComposableNode( + package='light_controlled_intersection_tactical_plugin', + plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', + name='light_controlled_intersection_tactical_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('route', env_log_levels) } + ], + # remappings = [ + # ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + # ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + # ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), + # ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), + # ("ui_instructions", [ EnvironmentVariable('CARMA_UI_NS', default_value=''), "/ui_instructions" ] ) + # ], + parameters=[ + vehicle_config_param_file + ] + ) ] ) diff --git a/light_controlled_intersection_tactical_plugin/CMakeLists.txt b/light_controlled_intersection_tactical_plugin/CMakeLists.txt index 7604e41ac6..0fda5e3faa 100644 --- a/light_controlled_intersection_tactical_plugin/CMakeLists.txt +++ b/light_controlled_intersection_tactical_plugin/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() # Name build targets +set(worker_lib light_controlled_intersection_tactical_plugin_worker_lib) set(node_exec light_controlled_intersection_tactical_plugin_node_exec) set(node_lib light_controlled_intersection_tactical_plugin_node) @@ -35,6 +36,10 @@ include_directories( ) # Build +ament_auto_add_library(${worker_lib} + src/light_controlled_intersection_tactical_plugin.cpp +) + ament_auto_add_library(${node_lib} SHARED src/light_controlled_intersection_tactical_plugin_node.cpp ) @@ -48,6 +53,10 @@ rclcpp_components_register_nodes(${node_lib} "light_controlled_intersection_tact # 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} +) + target_link_libraries(${node_exec} ${node_lib} ) diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp new file mode 100644 index 0000000000..b4f297abb3 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp @@ -0,0 +1,218 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp" + +/** + * \brief Macro definition to enable easier access to fields shared across the maneuver types + * \param mvr The maneuver object to invoke the accessors on + * \param property The name of the field to access on the specific maneuver types. Must be shared by all extant maneuver types + * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field + */ +#define GET_MANEUVER_PROPERTY(mvr, property)\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ + throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))) + +namespace light_controlled_intersection_tactical_plugin +{ + using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; + using GeneralTrajConfig = basic_autonomy::waypoint_generation::GeneralTrajConfig; + using DetailedTrajConfig = basic_autonomy::waypoint_generation::DetailedTrajConfig; + + enum TSCase { + CASE_1 = 1, + CASE_2 = 2, + CASE_3 = 3, + CASE_4 = 4, + CASE_5 = 5, + CASE_6 = 6, + CASE_7 = 7, + CASE_8 = 8, + }; + + struct TrajectoryParams { + double a1_ = 0; + double v1_ = 0; + double x1_ = 0; + + double a2_ = 0; + double v2_ = 0; + double x2_ = 0; + + double a3_ = 0; + double v3_ = 0; + double x3_ = 0; + }; + + /** + * \brief Class containing primary business logic for the Light Controlled Intersection Tactical Plugin + * + */ + class LightControlledIntersectionTacticalPlugin + { + + private: + // World Model object + carma_wm::WorldModelConstPtr wm_; + + // LightControlledIntersectionTacticalPlugin configuration + Config config_; + + // Logger for this object + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; + + // CARMA Streets Variables + // timestamp for msg received from carma streets + uint32_t street_msg_timestamp_ = 0.0; + // scheduled stop time + uint32_t scheduled_stop_time_ = 0.0; + // scheduled enter time + uint32_t scheduled_enter_time_ = 0.0; + // scheduled depart time + uint32_t scheduled_depart_time_ = 0.0; + // scheduled latest depart time + uint32_t scheduled_latest_depart_time_ = 0.0; + // flag to show if the vehicle is allowed in intersection + bool is_allowed_int_ = false; + + double speed_limit_ = 11.176; // Approximate speed limit; 25 mph by default + boost::optional last_case_; + boost::optional is_last_case_successful_; + carma_planning_msgs::msg::TrajectoryPlan last_trajectory_; + + carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later + + double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with + + // downtrack of host vehicle + double current_downtrack_ = 0.0; + + double last_successful_ending_downtrack_; // if algorithm was successful, this is traffic_light_downtrack + double last_successful_scheduled_entry_time_; // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) + + carma_planning_msgs::msg::Plugin plugin_discovery_msg_; + carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_; + std::vector last_final_speeds_; + + std::string light_controlled_intersection_strategy_ = "signalized"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends + + /** + * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. + * \param wm world_model pointer + * \param points_and_target_speeds of centerline points paired with speed limits whose speeds are to be modified: + * \param start_dist starting downtrack of the maneuver to be planned (excluding buffer points) in m + * \param remaining_dist distance for the maneuver to be planned (excluding buffer points) in m + * \param starting_speed starting speed at the start of the maneuver in m/s + * \param departure_speed ending speed of the maneuver a.k.a entry speed into the intersection m/s + * \param tsp trajectory smoothing parameters + * NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. + * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_ + */ + void apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + double starting_speed, double departure_speed, TrajectoryParams tsp); + + /** + * \brief Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals. + * Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing + * \param maneuver Maneuver associated that has starting downtrack and desired entry time + * \param starting_speed Starting speed of the vehicle + * \param points The set of points with raw speed limits whose speed profile to be changed. + * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. + * + */ + void apply_optimized_target_speed_profile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); + + /** + * \brief Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) + * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated raw speed limits + * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. + * If the first maneuver exceeds this then it's downtrack will be shifted to this value. + * \param wm Pointer to intialized world model for semantic map access + * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later + * \param state The vehicle state at the time the function is called + * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins + * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins + * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver + * NOTE: This function is a slightly modified version of the same function in basic_autonomy library and currently only plans for the first maneuver + */ + std::vector create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, + carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, + const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); + + /** + * \brief Given a Lanelet, find it's associated Speed Limit + * + * \param llt Constant Lanelet object + * + * \throw std::invalid_argument if the speed limit could not be retrieved + * + * \return value of speed limit in mps + */ + double findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const; + + public: + + /*! + * \brief LightControlledIntersectionTacticalPlugin constructor + */ + LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger); + + /** + * \brief Function to process the light controlled intersection tactical plugin service call for trajectory planning + * + * \param req The service request + * \param resp The service response + * + * \return True if success. False otherwise + */ + void plan_trajectory_cb( + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); + + }; + +} // light_controlled_intersection_tactical_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp index f284a67409..7d8a79970f 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp @@ -18,31 +18,10 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp" +#include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp" /** * \brief Macro definition to enable easier access to fields shared across the maneuver types @@ -59,35 +38,6 @@ namespace light_controlled_intersection_tactical_plugin { - using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; - using GeneralTrajConfig = basic_autonomy::waypoint_generation::GeneralTrajConfig; - using DetailedTrajConfig = basic_autonomy::waypoint_generation::DetailedTrajConfig; - - enum TSCase { - CASE_1 = 1, - CASE_2 = 2, - CASE_3 = 3, - CASE_4 = 4, - CASE_5 = 5, - CASE_6 = 6, - CASE_7 = 7, - CASE_8 = 8, - }; - - struct TrajectoryParams { - double a1_ = 0; - double v1_ = 0; - double x1_ = 0; - - double a2_ = 0; - double v2_ = 0; - double x2_ = 0; - - double a3_ = 0; - double v3_ = 0; - double x3_ = 0; - }; - /** * \brief ROS node for the LightControlledIntersectionTransitPluginNode * @@ -98,40 +48,7 @@ namespace light_controlled_intersection_tactical_plugin // LightControlledIntersectionTransitPluginNode configuration Config config_; - // CARMA Streets Variables - // timestamp for msg received from carma streets - uint32_t street_msg_timestamp_ = 0.0; - // scheduled stop time - uint32_t scheduled_stop_time_ = 0.0; - // scheduled enter time - uint32_t scheduled_enter_time_ = 0.0; - // scheduled depart time - uint32_t scheduled_depart_time_ = 0.0; - // scheduled latest depart time - uint32_t scheduled_latest_depart_time_ = 0.0; - // flag to show if the vehicle is allowed in intersection - bool is_allowed_int_ = false; - - double speed_limit_ = 11.176; // Approximate speed limit; 25 mph by default - boost::optional last_case_; - boost::optional is_last_case_successful_; - carma_planning_msgs::msg::TrajectoryPlan last_trajectory_; - - carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later - - double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with - - // downtrack of host vehicle - double current_downtrack_ = 0.0; - - double last_successful_ending_downtrack_; // if algorithm was successful, this is traffic_light_downtrack - double last_successful_scheduled_entry_time_; // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) - - carma_planning_msgs::msg::Plugin plugin_discovery_msg_; - carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_; - std::vector last_final_speeds_; - - std::string light_controlled_intersection_strategy_ = "signalized"; // Strategy carma-streets is sending. Could be more verbose but needs to be changed on both ends + std::shared_ptr worker_; public: @@ -146,60 +63,6 @@ namespace light_controlled_intersection_tactical_plugin rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector ¶meters); - /** - * \brief Creates a speed profile according to case one or two of the light controlled intersection, where the vehicle accelerates (then cruises if needed) and decelerates into the intersection. - * \param wm world_model pointer - * \param points_and_target_speeds of centerline points paired with speed limits whose speeds are to be modified: - * \param start_dist starting downtrack of the maneuver to be planned (excluding buffer points) in m - * \param remaining_dist distance for the maneuver to be planned (excluding buffer points) in m - * \param starting_speed starting speed at the start of the maneuver in m/s - * \param departure_speed ending speed of the maneuver a.k.a entry speed into the intersection m/s - * \param tsp trajectory smoothing parameters - * NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. - * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_ - */ - void apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, - double starting_speed, double departure_speed, TrajectoryParams tsp); - - /** - * \brief Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals. - * Based on TSMO USE CASE 2. Chapter 2. Trajectory Smoothing - * \param maneuver Maneuver associated that has starting downtrack and desired entry time - * \param starting_speed Starting speed of the vehicle - * \param points The set of points with raw speed limits whose speed profile to be changed. - * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. - * - */ - void apply_optimized_target_speed_profile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); - - /** - * \brief Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) - * \param maneuvers The list of maneuvers to convert to geometry points and calculate associated raw speed limits - * \param max_starting_downtrack The maximum downtrack that is allowed for the first maneuver. This should be set to the vehicle position or earlier. - * If the first maneuver exceeds this then it's downtrack will be shifted to this value. - * \param wm Pointer to intialized world model for semantic map access - * \param ending_state_before_buffer reference to Vehicle state, which is state before applying extra points for curvature calculation that are removed later - * \param state The vehicle state at the time the function is called - * \param general_config Basic autonomy struct defined to load general config parameters from tactical plugins - * \param detailed_config Basic autonomy struct defined to load detailed config parameters from tactical plugins - * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver - * NOTE: This function is a slightly modified version of the same function in basic_autonomy library and currently only plans for the first maneuver - */ - std::vector create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, - carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, - const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); - - /** - * \brief Given a Lanelet, find it's associated Speed Limit - * - * \param llt Constant Lanelet object - * - * \throw std::invalid_argument if the speed limit could not be retrieved - * - * \return value of speed limit in mps - */ - double findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const; - //// // Overrides //// diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp new file mode 100644 index 0000000000..4a1ecb6e44 --- /dev/null +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -0,0 +1,389 @@ +/* + * 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. + */ +#include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp" + +namespace light_controlled_intersection_tactical_plugin +{ + + + LightControlledIntersectionTacticalPlugin::LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config& config, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger) + :wm_(wm), config_(config), logger_(logger) + { + } + + void LightControlledIntersectionTacticalPlugin::plan_trajectory_cb( + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) + { + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Starting light controlled intersection trajectory planning"); + + if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size()) + { + throw std::invalid_argument( + "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); + } + std::vector maneuver_plan; + // expecting only one maneuver for an intersection + for(size_t i = req->maneuver_index_to_plan; i < req->maneuver_plan.maneuvers.size(); i++){ + + if(req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING + && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[i], parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_) + { + maneuver_plan.push_back(req->maneuver_plan.maneuvers[i]); + resp->related_maneuvers.push_back(i); + break; + } + else + { + throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver"); + } + } + + lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Planning state x:"<vehicle_state.x_pos_global <<" , y: " << req->vehicle_state.y_pos_global); + + current_downtrack_ = wm_->routeTrackPos(veh_pos).downtrack; + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Current_downtrack: "<< current_downtrack_); + + auto current_lanelets = wm_->getLaneletsFromPoint({ req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global}); + lanelet::ConstLanelet current_lanelet; + + if (current_lanelets.empty()) + { + RCLCPP_ERROR_STREAM(logger_->get_logger(), "Given vehicle position is not on the road! Returning..."); + return; + } + + // get the lanelet that is on the route in case overlapping ones found + for (auto llt : current_lanelets) + { + auto route = wm_->getRoute()->shortestPath(); + if (std::find(route.begin(), route.end(), llt) != route.end()) + { + current_lanelet = llt; + break; + } + } + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Current_lanelet: "<< current_lanelet.id()); + + speed_limit_ = findSpeedLimit(current_lanelet, wm_); + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "speed_limit_: "<< speed_limit_); + + DetailedTrajConfig wpg_detail_config; + GeneralTrajConfig wpg_general_config; + + wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("intersection_transit", + config_.default_downsample_ratio, + config_.turn_downsample_ratio); + + wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, + config_.curve_resample_step_size, config_.minimum_speed, + config_.vehicle_accel_limit, + config_.lateral_accel_limit, + config_.speed_moving_average_window_size, + config_.curvature_moving_average_window_size, config_.back_distance, + config_.buffer_ending_downtrack); + + + // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver + auto points_and_target_speeds = create_geometry_profile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance), + wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config); + + // Change raw speed limit values to target speeds specified by the algorithm + apply_optimized_target_speed_profile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel, points_and_target_speeds); + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "points_and_target_speeds: " << points_and_target_speeds.size()); + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "PlanTrajectory"); + + //Trajectory Plan + carma_planning_msgs::msg::TrajectoryPlan trajectory; + trajectory.header.frame_id = "map"; + trajectory.header.stamp = req->header.stamp; + trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); + + // Compose smooth trajectory/speed by resampling + trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, + req->vehicle_state, req->header.stamp, wm_, ending_state_before_buffer_, debug_msg_, + wpg_detail_config); // Compute the trajectory + + // Set the planning plugin field name + for (auto& p : trajectory.trajectory_points) { + p.planner_plugin_name = plugin_discovery_msg_.name; + } + + bool is_new_case_successful = GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[1]); + TSCase new_case = static_castGET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[0]); + + if (is_last_case_successful_ != boost::none && last_case_ != boost::none) + { + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "all variables are set!"); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get()); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_); + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "evaluation time: " << std::to_string(last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds())); + } + else + { + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Not all variables are set..."); + } + + carma_planning_msgs::msg::TrajectoryPlan reduced_last_traj; + std::vector reduced_final_speeds; + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "traj points size: " << last_trajectory_.trajectory_points.size() << ", last_final_speeds_ size: " << + last_final_speeds_.size() ); + + for (size_t i = 0; i < last_trajectory_.trajectory_points.size(); i++) + { + if (rclcpp::Time(last_trajectory_.trajectory_points[i].target_time) > rclcpp::Time(req->header.stamp) - rclcpp::Duration(0.1 * 1e9)) // Duration is in nanoseconds + { + reduced_last_traj.trajectory_points.emplace_back(last_trajectory_.trajectory_points[i]); + reduced_final_speeds.emplace_back(last_final_speeds_[i]); + } + } + + last_final_speeds_ = reduced_final_speeds; + last_trajectory_.trajectory_points = reduced_last_traj.trajectory_points; + + if (is_last_case_successful_ != boost::none && last_case_ != boost::none + && last_case_.get() == new_case + && is_new_case_successful == true + && !last_trajectory_.trajectory_points.empty() + && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp) + rclcpp::Duration(1 * 1e9)) // Duration is in nanoseconds + { + resp->trajectory_plan = last_trajectory_; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Last TRAJ's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING LAST TRAJ: " << (int)last_case_.get()); + } + else if (is_last_case_successful_ != boost::none && last_case_ != boost::none + && is_last_case_successful_.get() == true + && is_new_case_successful == false + && last_successful_ending_downtrack_ - current_downtrack_ < config_.algorithm_evaluation_distance + && last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds() < config_.algorithm_evaluation_period + && !last_trajectory_.trajectory_points.empty() + && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() > rclcpp::Time(req->header.stamp).seconds()) + { + resp->trajectory_plan = last_trajectory_; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Last Traj's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds() << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_)); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get()); + } + else + { + last_trajectory_ = trajectory; + resp->trajectory_plan = trajectory; + last_case_ = new_case; + last_final_speeds_ = debug_msg_.velocity_profile; + is_last_case_successful_ = is_new_case_successful; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING NEW: Target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); + if (is_new_case_successful) + { + last_successful_ending_downtrack_ = GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist); // if algorithm was successful, this is traffic_light_downtrack + last_successful_scheduled_entry_time_ = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds(); // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_)); + } + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING NEW CASE!!! : " << (int)last_case_.get()); + + } + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful); + + resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front(); + + resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); + + return; + } + + void LightControlledIntersectionTacticalPlugin::apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + double starting_speed, double departure_speed, TrajectoryParams tsp) + { + if (points_and_target_speeds.empty()) + { + throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile..."); + } + + // Checking route geometry start against start_dist and adjust profile + double planning_downtrack_start = wm->routeTrackPos(points_and_target_speeds[0].point).downtrack; // this can include buffered points earlier than maneuver start_dist + + //Check calculated total dist against maneuver limits + double total_distance_needed = remaining_dist; + double dist1 = tsp.x1_ - start_dist; + double dist2 = tsp.x2_ - start_dist; + double dist3 = tsp.x3_ - start_dist; + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "total_distance_needed: " << total_distance_needed << "\n" << + "dist1: " << dist1 << "\n" << + "dist2: " << dist2 << "\n" << + "dist3: " << dist3); + double algo_min_speed = std::min({tsp.v1_,tsp.v2_,tsp.v3_}); + double algo_max_speed = std::max({tsp.v1_,tsp.v2_,tsp.v3_}); + + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "found algo_minimum_speed: " << algo_min_speed << "\n" << + "algo_max_speed: " << algo_max_speed); + + double total_dist_planned = 0; //Starting dist for maneuver treated as 0.0 + + if (planning_downtrack_start < start_dist) + { + //Account for the buffer distance that is technically not part of this maneuver + + total_dist_planned = planning_downtrack_start - start_dist; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); + } + + double prev_speed = starting_speed; + auto prev_point = points_and_target_speeds.front(); + + for(auto& p : points_and_target_speeds) + { + double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point); + total_dist_planned += delta_d; + + //Apply the speed from algorithm at dist covered + //Kinematic: v_f = sqrt(v_o^2 + 2*a*d) + double speed_i; + if (total_dist_planned <= epsilon_) + { + //Keep target speed same for buffer distance portion + speed_i = starting_speed; + } + else if(total_dist_planned <= dist1 + epsilon_){ + //First segment + speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.a1_ * total_dist_planned); + } + else if(total_dist_planned > dist1 && total_dist_planned <= dist2 + epsilon_){ + //Second segment + speed_i = sqrt(std::max(pow(tsp.v1_, 2) + 2 * tsp.a2_ * (total_dist_planned - dist1), 0.0)); //std::max to ensure negative value is not sqrt + } + else if (total_dist_planned > dist2 && total_dist_planned <= dist3 + epsilon_) + { + //Third segment + speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt + } + else + { + //buffer points that will be cut + speed_i = prev_speed; + } + + if (isnan(speed_i)) + { + speed_i = std::max(config_.minimum_speed, algo_min_speed); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Detected nan number from equations. Set to " << speed_i); + } + + p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed}); + p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned); + + prev_point = p; + prev_speed = p.speed; + } + } + + void LightControlledIntersectionTacticalPlugin::apply_optimized_target_speed_profile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds) + { + if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || + GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){ + throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters."); + } + + TrajectoryParams tsp; + + tsp.a1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]); + tsp.v1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); + tsp.x1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]); + + tsp.a2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]); + tsp.v2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]); + tsp.x2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[5]); + + tsp.a3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[6]); + tsp.v3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[7]); + tsp.x3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[8]); + + double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); + double ending_downtrack = GET_MANEUVER_PROPERTY(maneuver, end_dist); + double departure_speed = GET_MANEUVER_PROPERTY(maneuver, end_speed); + double scheduled_entry_time = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time)).seconds(); + double entry_dist = ending_downtrack - starting_downtrack; + + // change speed profile depending on algorithm case starting from maneuver start_dist + apply_trajectory_smoothing_algorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, + departure_speed, tsp); + } + + double LightControlledIntersectionTacticalPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const + { + lanelet::Optional traffic_rules = wm->getTrafficRules(); + if (traffic_rules) + { + return (*traffic_rules)->speedLimit(llt).speedLimit.value(); + } + else + { + throw std::invalid_argument("Valid traffic rules object could not be built"); + } + } + + std::vector LightControlledIntersectionTacticalPlugin::create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, + carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, + const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config) + { + std::vector points_and_target_speeds; + + bool first = true; + std::unordered_set visited_lanelets; + std::vector processed_maneuvers; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "VehDowntrack: "<get_logger(), "Used downtrack: " << starting_downtrack); + + // check if required parameter from strategic planner is present + if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).empty()) + { + throw std::invalid_argument("No time_to_schedule_entry is provided in float_valued_meta_data"); + } + + //overwrite maneuver type to use lane follow library function + carma_planning_msgs::msg::Maneuver temp_maneuver = maneuver; + temp_maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Creating Lane Follow Geometry"); + std::vector lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); + points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + + break; // expected to receive only one maneuver to plan + } + + //Add buffer ending to lane follow points at the end of maneuver(s) end dist + if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ + points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config); + } + + return points_and_target_speeds; + } + +} // light_controlled_intersection_tactical_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index 098615299a..070710e816 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -55,21 +55,21 @@ namespace light_controlled_intersection_tactical_plugin rcl_interfaces::msg::SetParametersResult LightControlledIntersectionTransitPluginNode::parameter_update_callback(const std::vector ¶meters) { auto error = update_params( - {{"centerline_sampling_spacing", config_.centerline_sampling_spacing}}, parameters); - // {"trajectory_time_length", config_.trajectory_time_length}, - // {"curve_resample_step_size", config_.curve_resample_step_size}, - // {"back_distance", config_.back_distance}, - // {"buffer_ending_downtrack", config_.buffer_ending_downtrack}, - // {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier}, - // {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier}, - // {"lat_accel_multiplier", config_.lat_accel_multiplier}, - // {"stop_line_buffer", config_.stop_line_buffer}, - // {"minimum_speed", config_.minimum_speed}, - // {"algorithm_evaluation_distance", config_.algorithm_evaluation_distance}, - // {"algorithm_evaluation_period", config_.algorithm_evaluation_period}, - // {"vehicle_lateral_accel_limit", config_.lateral_accel_limit}, - // {"vehicle_acceleration_limit", config_.vehicle_accel_limit}, - // {"vehicle_deceleration_limit", config_.vehicle_decel_limit}}, parameters); + {{"centerline_sampling_spacing", config_.centerline_sampling_spacing}, + {"trajectory_time_length", config_.trajectory_time_length}, + {"curve_resample_step_size", config_.curve_resample_step_size}, + {"back_distance", config_.back_distance}, + {"buffer_ending_downtrack", config_.buffer_ending_downtrack}, + {"vehicle_decel_limit_multiplier", config_.vehicle_decel_limit_multiplier}, + {"vehicle_accel_limit_multiplier", config_.vehicle_accel_limit_multiplier}, + {"lat_accel_multiplier", config_.lat_accel_multiplier}, + {"stop_line_buffer", config_.stop_line_buffer}, + {"minimum_speed", config_.minimum_speed}, + {"algorithm_evaluation_distance", config_.algorithm_evaluation_distance}, + {"algorithm_evaluation_period", config_.algorithm_evaluation_period}, + {"vehicle_lateral_accel_limit", config_.lateral_accel_limit}, + {"vehicle_acceleration_limit", config_.vehicle_accel_limit}, + {"vehicle_deceleration_limit", config_.vehicle_decel_limit}}, parameters); auto error_2 = update_params( {{"default_downsample_ratio", config_.default_downsample_ratio}, {"turn_downsample_ratio", config_.turn_downsample_ratio}, @@ -116,6 +116,9 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_); + // Initialize worker object + worker_ = std::make_shared(get_world_model(), config_, get_node_logging_interface()); + // Return success if everything initialized successfully return CallbackReturn::SUCCESS; } @@ -133,361 +136,7 @@ namespace light_controlled_intersection_tactical_plugin carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { - RCLCPP_DEBUG_STREAM(get_logger(), "Starting light controlled intersection trajectory planning"); - - if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size()) - { - throw std::invalid_argument( - "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + - " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); - } - std::vector maneuver_plan; - // expecting only one maneuver for an intersection - for(size_t i = req->maneuver_index_to_plan; i < req->maneuver_plan.maneuvers.size(); i++){ - - if(req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING - && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[i], parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_) - { - maneuver_plan.push_back(req->maneuver_plan.maneuvers[i]); - resp->related_maneuvers.push_back(i); - break; - } - else - { - throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver"); - } - } - - lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global); - RCLCPP_DEBUG_STREAM(get_logger(), "Planning state x:"<vehicle_state.x_pos_global <<" , y: " << req->vehicle_state.y_pos_global); - - current_downtrack_ = get_world_model()->routeTrackPos(veh_pos).downtrack; - - RCLCPP_DEBUG_STREAM(get_logger(), "Current_downtrack: "<< current_downtrack_); - - auto current_lanelets = get_world_model()->getLaneletsFromPoint({ req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global}); - lanelet::ConstLanelet current_lanelet; - - if (current_lanelets.empty()) - { - RCLCPP_ERROR_STREAM(get_logger(), "Given vehicle position is not on the road! Returning..."); - return; - } - - // get the lanelet that is on the route in case overlapping ones found - for (auto llt : current_lanelets) - { - auto route = get_world_model()->getRoute()->shortestPath(); - if (std::find(route.begin(), route.end(), llt) != route.end()) - { - current_lanelet = llt; - break; - } - } - - RCLCPP_DEBUG_STREAM(get_logger(), "Current_lanelet: "<< current_lanelet.id()); - - speed_limit_ = findSpeedLimit(current_lanelet, get_world_model()); - - RCLCPP_DEBUG_STREAM(get_logger(), "speed_limit_: "<< speed_limit_); - - DetailedTrajConfig wpg_detail_config; - GeneralTrajConfig wpg_general_config; - - wpg_general_config = basic_autonomy:: waypoint_generation::compose_general_trajectory_config("intersection_transit", - config_.default_downsample_ratio, - config_.turn_downsample_ratio); - - wpg_detail_config = basic_autonomy:: waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, - config_.curve_resample_step_size, config_.minimum_speed, - config_.vehicle_accel_limit, - config_.lateral_accel_limit, - config_.speed_moving_average_window_size, - config_.curvature_moving_average_window_size, config_.back_distance, - config_.buffer_ending_downtrack); - - - // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver - auto points_and_target_speeds = create_geometry_profile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance), - get_world_model(), ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config); - - // Change raw speed limit values to target speeds specified by the algorithm - apply_optimized_target_speed_profile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel, points_and_target_speeds); - - RCLCPP_DEBUG_STREAM(get_logger(), "points_and_target_speeds: " << points_and_target_speeds.size()); - - RCLCPP_DEBUG_STREAM(get_logger(), "PlanTrajectory"); - - //Trajectory Plan - carma_planning_msgs::msg::TrajectoryPlan trajectory; - trajectory.header.frame_id = "map"; - trajectory.header.stamp = req->header.stamp; - trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); - - // Compose smooth trajectory/speed by resampling - trajectory.trajectory_points = basic_autonomy::waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, - req->vehicle_state, req->header.stamp, get_world_model(), ending_state_before_buffer_, debug_msg_, - wpg_detail_config); // Compute the trajectory - - // Set the planning plugin field name - for (auto& p : trajectory.trajectory_points) { - p.planner_plugin_name = plugin_discovery_msg_.name; - } - - bool is_new_case_successful = GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[1]); - TSCase new_case = static_castGET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[0]); - - if (is_last_case_successful_ != boost::none && last_case_ != boost::none) - { - RCLCPP_DEBUG_STREAM(get_logger(), "all variables are set!"); - RCLCPP_DEBUG_STREAM(get_logger(), "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get()); - RCLCPP_DEBUG_STREAM(get_logger(), "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_); - - RCLCPP_DEBUG_STREAM(get_logger(), "evaluation time: " << std::to_string(last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds())); - } - else - { - RCLCPP_DEBUG_STREAM(get_logger(), "Not all variables are set..."); - } - - carma_planning_msgs::msg::TrajectoryPlan reduced_last_traj; - std::vector reduced_final_speeds; - - RCLCPP_DEBUG_STREAM(get_logger(), "traj points size: " << last_trajectory_.trajectory_points.size() << ", last_final_speeds_ size: " << - last_final_speeds_.size() ); - - for (size_t i = 0; i < last_trajectory_.trajectory_points.size(); i++) - { - if (rclcpp::Time(last_trajectory_.trajectory_points[i].target_time) > rclcpp::Time(req->header.stamp) - rclcpp::Duration(0.1 * 1e9)) // Duration is in nanoseconds - { - reduced_last_traj.trajectory_points.emplace_back(last_trajectory_.trajectory_points[i]); - reduced_final_speeds.emplace_back(last_final_speeds_[i]); - } - } - - last_final_speeds_ = reduced_final_speeds; - last_trajectory_.trajectory_points = reduced_last_traj.trajectory_points; - - if (is_last_case_successful_ != boost::none && last_case_ != boost::none - && last_case_.get() == new_case - && is_new_case_successful == true - && !last_trajectory_.trajectory_points.empty() - && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp) + rclcpp::Duration(1 * 1e9)) // Duration is in nanoseconds - { - resp->trajectory_plan = last_trajectory_; - RCLCPP_DEBUG_STREAM(get_logger(), "Last TRAJ's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); - RCLCPP_DEBUG_STREAM(get_logger(), "USING LAST TRAJ: " << (int)last_case_.get()); - } - else if (is_last_case_successful_ != boost::none && last_case_ != boost::none - && is_last_case_successful_.get() == true - && is_new_case_successful == false - && last_successful_ending_downtrack_ - current_downtrack_ < config_.algorithm_evaluation_distance - && last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds() < config_.algorithm_evaluation_period - && !last_trajectory_.trajectory_points.empty() - && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() > rclcpp::Time(req->header.stamp).seconds()) - { - resp->trajectory_plan = last_trajectory_; - RCLCPP_DEBUG_STREAM(get_logger(), "Last Traj's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds() << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_)); - RCLCPP_DEBUG_STREAM(get_logger(), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get()); - } - else - { - last_trajectory_ = trajectory; - resp->trajectory_plan = trajectory; - last_case_ = new_case; - last_final_speeds_ = debug_msg_.velocity_profile; - is_last_case_successful_ = is_new_case_successful; - RCLCPP_DEBUG_STREAM(get_logger(), "USING NEW: Target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); - if (is_new_case_successful) - { - last_successful_ending_downtrack_ = GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist); // if algorithm was successful, this is traffic_light_downtrack - last_successful_scheduled_entry_time_ = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds(); // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) - RCLCPP_DEBUG_STREAM(get_logger(), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_)); - } - RCLCPP_DEBUG_STREAM(get_logger(), "USING NEW CASE!!! : " << (int)last_case_.get()); - - } - RCLCPP_DEBUG_STREAM(get_logger(), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful); - - resp->trajectory_plan.initial_longitudinal_velocity = last_final_speeds_.front(); - - resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); - - return; - } - - void LightControlledIntersectionTransitPluginNode::apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, - double starting_speed, double departure_speed, TrajectoryParams tsp) - { - if (points_and_target_speeds.empty()) - { - throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile..."); - } - - // Checking route geometry start against start_dist and adjust profile - double planning_downtrack_start = get_world_model()->routeTrackPos(points_and_target_speeds[0].point).downtrack; // this can include buffered points earlier than maneuver start_dist - - //Check calculated total dist against maneuver limits - double total_distance_needed = remaining_dist; - double dist1 = tsp.x1_ - start_dist; - double dist2 = tsp.x2_ - start_dist; - double dist3 = tsp.x3_ - start_dist; - - RCLCPP_DEBUG_STREAM(get_logger(), "total_distance_needed: " << total_distance_needed << "\n" << - "dist1: " << dist1 << "\n" << - "dist2: " << dist2 << "\n" << - "dist3: " << dist3); - double algo_min_speed = std::min({tsp.v1_,tsp.v2_,tsp.v3_}); - double algo_max_speed = std::max({tsp.v1_,tsp.v2_,tsp.v3_}); - - RCLCPP_DEBUG_STREAM(get_logger(), "found algo_minimum_speed: " << algo_min_speed << "\n" << - "algo_max_speed: " << algo_max_speed); - - double total_dist_planned = 0; //Starting dist for maneuver treated as 0.0 - - if (planning_downtrack_start < start_dist) - { - //Account for the buffer distance that is technically not part of this maneuver - - total_dist_planned = planning_downtrack_start - start_dist; - RCLCPP_DEBUG_STREAM(get_logger(), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); - } - - double prev_speed = starting_speed; - auto prev_point = points_and_target_speeds.front(); - - for(auto& p : points_and_target_speeds) - { - double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point); - total_dist_planned += delta_d; - - //Apply the speed from algorithm at dist covered - //Kinematic: v_f = sqrt(v_o^2 + 2*a*d) - double speed_i; - if (total_dist_planned <= epsilon_) - { - //Keep target speed same for buffer distance portion - speed_i = starting_speed; - } - else if(total_dist_planned <= dist1 + epsilon_){ - //First segment - speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.a1_ * total_dist_planned); - } - else if(total_dist_planned > dist1 && total_dist_planned <= dist2 + epsilon_){ - //Second segment - speed_i = sqrt(std::max(pow(tsp.v1_, 2) + 2 * tsp.a2_ * (total_dist_planned - dist1), 0.0)); //std::max to ensure negative value is not sqrt - } - else if (total_dist_planned > dist2 && total_dist_planned <= dist3 + epsilon_) - { - //Third segment - speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt - } - else - { - //buffer points that will be cut - speed_i = prev_speed; - } - - if (isnan(speed_i)) - { - speed_i = std::max(config_.minimum_speed, algo_min_speed); - RCLCPP_DEBUG_STREAM(get_logger(), "Detected nan number from equations. Set to " << speed_i); - } - - p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed}); - p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); - RCLCPP_DEBUG_STREAM(get_logger(), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned); - - prev_point = p; - prev_speed = p.speed; - } - } - - void LightControlledIntersectionTransitPluginNode::apply_optimized_target_speed_profile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds) - { - if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || - GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){ - throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters."); - } - - TrajectoryParams tsp; - - tsp.a1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[0]); - tsp.v1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[1]); - tsp.x1_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[2]); - - tsp.a2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[3]); - tsp.v2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[4]); - tsp.x2_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[5]); - - tsp.a3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[6]); - tsp.v3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[7]); - tsp.x3_ = GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data[8]); - - double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); - double ending_downtrack = GET_MANEUVER_PROPERTY(maneuver, end_dist); - double departure_speed = GET_MANEUVER_PROPERTY(maneuver, end_speed); - double scheduled_entry_time = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time)).seconds(); - double entry_dist = ending_downtrack - starting_downtrack; - - // change speed profile depending on algorithm case starting from maneuver start_dist - apply_trajectory_smoothing_algorithm(get_world_model(), points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, - departure_speed, tsp); - } - - double LightControlledIntersectionTransitPluginNode::findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const - { - lanelet::Optional traffic_rules = wm->getTrafficRules(); - if (traffic_rules) - { - return (*traffic_rules)->speedLimit(llt).speedLimit.value(); - } - else - { - throw std::invalid_argument("Valid traffic rules object could not be built"); - } - } - - std::vector LightControlledIntersectionTransitPluginNode::create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, - carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, - const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config) - { - std::vector points_and_target_speeds; - - bool first = true; - std::unordered_set visited_lanelets; - std::vector processed_maneuvers; - RCLCPP_DEBUG_STREAM(get_logger(), "VehDowntrack: "< lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); - - break; // expected to receive only one maneuver to plan - } - - //Add buffer ending to lane follow points at the end of maneuver(s) end dist - if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ - points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config); - } - - return points_and_target_speeds; + worker_->plan_trajectory_cb(req, resp); } } // light_controlled_intersection_tactical_plugin diff --git a/light_controlled_intersection_tactical_plugin/test/node_test.cpp b/light_controlled_intersection_tactical_plugin/test/node_test.cpp index c6be5e54e3..51eddba9b8 100644 --- a/light_controlled_intersection_tactical_plugin/test/node_test.cpp +++ b/light_controlled_intersection_tactical_plugin/test/node_test.cpp @@ -22,16 +22,19 @@ #include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp" +namespace light_controlled_intersection_transit_plugin +{ + + TEST(LCITacticalPluginTest, apply_accel_cruise_decel_speed_profile_test) + { + rclcpp::NodeOptions options; + auto lci_node = std::make_shared(options); + lci_node->configure(); // Call configure state transition + lci_node->activate(); // Call activate state transition + } -// TODO for USER: Implement a real test using GTest -TEST(Testlight_controlled_intersection_tactical_plugin, example_test){ - - rclcpp::NodeOptions options; - auto worker_node = std::make_shared(options); +} // namespace light_controlled_intersection_transit_plugin - worker_node->configure(); //Call configure state transition - worker_node->activate(); //Call activate state transition to get not read for runtime -} int main(int argc, char ** argv) { From c6697dc64899a7c15dd77f5fb5a0f9c495a59c6a Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Wed, 17 Aug 2022 07:52:20 -0700 Subject: [PATCH 068/165] Fixes for launching lci tactical plugin --- carma/launch/guidance.launch.py | 23 ++++++----- ...ed_intersection_tactical_plugin_config.hpp | 38 +++++++++---------- ...led_intersection_tactical_plugin_launch.py | 2 +- 3 files changed, 33 insertions(+), 30 deletions(-) diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index 3385a9b233..865bd0d8da 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -82,6 +82,9 @@ def generate_launch_description(): trajectory_visualizer_param_file = os.path.join( get_package_share_directory('trajectory_visualizer'), 'config/parameters.yaml') + light_controlled_intersection_tactical_plugin_param_file = os.path.join( + get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') + env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') subsystem_controller_param_file = LaunchConfiguration('subsystem_controller_param_file') @@ -251,20 +254,20 @@ def generate_launch_description(): ComposableNode( package='light_controlled_intersection_tactical_plugin', plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', - name='light_controlled_intersection_tactical_plugin_node', + name='light_controlled_intersection_tactical_plugin', extra_arguments=[ {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('route', env_log_levels) } + {'--log-level' : GetLogLevel('light_controlled_intersection_tactical_plugin', env_log_levels) } + ], + 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" ] ), + ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ) ], - # remappings = [ - # ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), - # ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), - # ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), - # ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), - # ("ui_instructions", [ EnvironmentVariable('CARMA_UI_NS', default_value=''), "/ui_instructions" ] ) - # ], parameters=[ - vehicle_config_param_file + vehicle_config_param_file, + light_controlled_intersection_tactical_plugin_param_file ] ) ] diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp index 1ef6d9dea7..430753e94f 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp @@ -27,25 +27,25 @@ namespace light_controlled_intersection_tactical_plugin */ struct Config { - double centerline_sampling_spacing; - double trajectory_time_length; - int default_downsample_ratio; - int turn_downsample_ratio; - double curve_resample_step_size; - int curvature_moving_average_window_size; - int speed_moving_average_window_size; - double back_distance; - double buffer_ending_downtrack; - double vehicle_decel_limit_multiplier; - double vehicle_accel_limit_multiplier; - double lat_accel_multiplier; - double stop_line_buffer; - double minimum_speed; - double algorithm_evaluation_distance; - double algorithm_evaluation_period; - double lateral_accel_limit; - double vehicle_accel_limit; - double vehicle_decel_limit; + double centerline_sampling_spacing = 1.0; + double trajectory_time_length = 12.0; + int default_downsample_ratio = 10; + int turn_downsample_ratio = 5; + double curve_resample_step_size = 1.0; + int curvature_moving_average_window_size = 9; + int speed_moving_average_window_size = 5; + double back_distance = 20.0; + double buffer_ending_downtrack = 40.0; + double vehicle_decel_limit_multiplier = 1.0; + double vehicle_accel_limit_multiplier = 1.0; + double lat_accel_multiplier = 1.0; + double stop_line_buffer = 0.50; + double minimum_speed = 4.4704; + double algorithm_evaluation_distance = 35.0; + double algorithm_evaluation_period = 4.5; + double lateral_accel_limit = 2.5; + double vehicle_accel_limit = 2.0; + double vehicle_decel_limit = 2.0; // Stream operator for this config friend std::ostream &operator<<(std::ostream &output, const Config &c) diff --git a/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py b/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py index 4d2812a336..6fd044ddaf 100644 --- a/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py +++ b/light_controlled_intersection_tactical_plugin/launch/light_controlled_intersection_tactical_plugin_launch.py @@ -52,7 +52,7 @@ def generate_launch_description(): ComposableNode( package='light_controlled_intersection_tactical_plugin', plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', - name='light_controlled_intersection_tactical_plugin_node', + name='light_controlled_intersection_tactical_plugin', extra_arguments=[ {'use_intra_process_comms': True}, {'--log-level' : log_level } From 042424d2fd90c5ea3f1a1e7c3fe4ce31342debcb Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Wed, 17 Aug 2022 08:04:23 -0700 Subject: [PATCH 069/165] Code cleanup --- ..._controlled_intersection_tactical_plugin.hpp | 12 +----------- ...rolled_intersection_tactical_plugin_node.hpp | 17 +++-------------- .../package.xml | 3 +-- ...rolled_intersection_tactical_plugin_node.cpp | 2 +- 4 files changed, 6 insertions(+), 28 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp index b4f297abb3..f9e682217d 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp @@ -23,15 +23,8 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include -#include #include #include #include @@ -98,7 +91,7 @@ namespace light_controlled_intersection_tactical_plugin // World Model object carma_wm::WorldModelConstPtr wm_; - // LightControlledIntersectionTacticalPlugin configuration + // Config for this object Config config_; // Logger for this object @@ -203,11 +196,8 @@ namespace light_controlled_intersection_tactical_plugin /** * \brief Function to process the light controlled intersection tactical plugin service call for trajectory planning - * * \param req The service request * \param resp The service response - * - * \return True if success. False otherwise */ void plan_trajectory_cb( carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp index 7d8a79970f..867e594c1d 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp @@ -23,19 +23,6 @@ #include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_config.hpp" #include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp" -/** - * \brief Macro definition to enable easier access to fields shared across the maneuver types - * \param mvr The maneuver object to invoke the accessors on - * \param property The name of the field to access on the specific maneuver types. Must be shared by all extant maneuver types - * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field - */ -#define GET_MANEUVER_PROPERTY(mvr, property)\ - (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ - ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ - ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ - ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ - throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))) - namespace light_controlled_intersection_tactical_plugin { /** @@ -45,9 +32,11 @@ namespace light_controlled_intersection_tactical_plugin class LightControlledIntersectionTransitPluginNode : public carma_guidance_plugins::TacticalPlugin { private: - // LightControlledIntersectionTransitPluginNode configuration + // Config for this object Config config_; + + // Worker object std::shared_ptr worker_; public: diff --git a/light_controlled_intersection_tactical_plugin/package.xml b/light_controlled_intersection_tactical_plugin/package.xml index 24a1557dda..67231fc320 100644 --- a/light_controlled_intersection_tactical_plugin/package.xml +++ b/light_controlled_intersection_tactical_plugin/package.xml @@ -15,7 +15,7 @@ light_controlled_intersection_tactical_plugin - 1.0.0 + 4.0.0 The light_controlled_intersection_tactical_plugin package carma @@ -33,7 +33,6 @@ carma_guidance_plugins carma_debug_ros2_msgs lanelet2_core - trajectory_utils_ros2 basic_autonomy_ros2 ament_lint_auto diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index 070710e816..d4c780e9c2 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -128,7 +128,7 @@ namespace light_controlled_intersection_tactical_plugin } std::string LightControlledIntersectionTransitPluginNode::get_version_id() { - return "v1.0"; + return "v4.0"; // Version ID matches the value set in this package's package.xml } void LightControlledIntersectionTransitPluginNode::plan_trajectory_callback( From 01a1a766a77d55f36a7e34ee29eb6fb226840b7a Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Wed, 17 Aug 2022 08:19:38 -0700 Subject: [PATCH 070/165] fix indentation --- ...ontrolled_intersection_tactical_plugin.cpp | 210 +++++++++--------- 1 file changed, 105 insertions(+), 105 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 4a1ecb6e44..4a864e513d 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -34,8 +34,8 @@ namespace light_controlled_intersection_tactical_plugin if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size()) { throw std::invalid_argument( - "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + - " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); + "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); } std::vector maneuver_plan; // expecting only one maneuver for an intersection @@ -55,13 +55,13 @@ namespace light_controlled_intersection_tactical_plugin } lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Planning state x:"<vehicle_state.x_pos_global <<" , y: " << req->vehicle_state.y_pos_global); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Planning state x:" << req->vehicle_state.x_pos_global << " , y: " << req->vehicle_state.y_pos_global); current_downtrack_ = wm_->routeTrackPos(veh_pos).downtrack; RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Current_downtrack: "<< current_downtrack_); - auto current_lanelets = wm_->getLaneletsFromPoint({ req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global}); + auto current_lanelets = wm_->getLaneletsFromPoint({req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global}); lanelet::ConstLanelet current_lanelet; if (current_lanelets.empty()) @@ -81,11 +81,11 @@ namespace light_controlled_intersection_tactical_plugin } } - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Current_lanelet: "<< current_lanelet.id()); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Current_lanelet: " << current_lanelet.id()); speed_limit_ = findSpeedLimit(current_lanelet, wm_); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "speed_limit_: "<< speed_limit_); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "speed_limit_: " << speed_limit_); DetailedTrajConfig wpg_detail_config; GeneralTrajConfig wpg_general_config; @@ -127,7 +127,7 @@ namespace light_controlled_intersection_tactical_plugin // Set the planning plugin field name for (auto& p : trajectory.trajectory_points) { - p.planner_plugin_name = plugin_discovery_msg_.name; + p.planner_plugin_name = plugin_discovery_msg_.name; } bool is_new_case_successful = GET_MANEUVER_PROPERTY(maneuver_plan.front(), parameters.int_valued_meta_data[1]); @@ -135,15 +135,15 @@ namespace light_controlled_intersection_tactical_plugin if (is_last_case_successful_ != boost::none && last_case_ != boost::none) { - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "all variables are set!"); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get()); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "all variables are set!"); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "is_last_case_successful_.get(): " << (int)is_last_case_successful_.get()); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "evaluation distance: " << last_successful_ending_downtrack_ - current_downtrack_); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "evaluation time: " << std::to_string(last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds())); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "evaluation time: " << std::to_string(last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds())); } else { - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Not all variables are set..."); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Not all variables are set..."); } carma_planning_msgs::msg::TrajectoryPlan reduced_last_traj; @@ -154,11 +154,11 @@ namespace light_controlled_intersection_tactical_plugin for (size_t i = 0; i < last_trajectory_.trajectory_points.size(); i++) { - if (rclcpp::Time(last_trajectory_.trajectory_points[i].target_time) > rclcpp::Time(req->header.stamp) - rclcpp::Duration(0.1 * 1e9)) // Duration is in nanoseconds - { - reduced_last_traj.trajectory_points.emplace_back(last_trajectory_.trajectory_points[i]); - reduced_final_speeds.emplace_back(last_final_speeds_[i]); - } + if (rclcpp::Time(last_trajectory_.trajectory_points[i].target_time) > rclcpp::Time(req->header.stamp) - rclcpp::Duration(0.1 * 1e9)) // Duration is in nanoseconds + { + reduced_last_traj.trajectory_points.emplace_back(last_trajectory_.trajectory_points[i]); + reduced_final_speeds.emplace_back(last_final_speeds_[i]); + } } last_final_speeds_ = reduced_final_speeds; @@ -170,37 +170,37 @@ namespace light_controlled_intersection_tactical_plugin && !last_trajectory_.trajectory_points.empty() && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time) > rclcpp::Time(req->header.stamp) + rclcpp::Duration(1 * 1e9)) // Duration is in nanoseconds { - resp->trajectory_plan = last_trajectory_; - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Last TRAJ's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING LAST TRAJ: " << (int)last_case_.get()); + resp->trajectory_plan = last_trajectory_; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Last TRAJ's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING LAST TRAJ: " << (int)last_case_.get()); } else if (is_last_case_successful_ != boost::none && last_case_ != boost::none - && is_last_case_successful_.get() == true - && is_new_case_successful == false - && last_successful_ending_downtrack_ - current_downtrack_ < config_.algorithm_evaluation_distance - && last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds() < config_.algorithm_evaluation_period - && !last_trajectory_.trajectory_points.empty() - && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() > rclcpp::Time(req->header.stamp).seconds()) + && is_last_case_successful_.get() == true + && is_new_case_successful == false + && last_successful_ending_downtrack_ - current_downtrack_ < config_.algorithm_evaluation_distance + && last_successful_scheduled_entry_time_ - rclcpp::Time(req->header.stamp).seconds() < config_.algorithm_evaluation_period + && !last_trajectory_.trajectory_points.empty() + && rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() > rclcpp::Time(req->header.stamp).seconds()) { - resp->trajectory_plan = last_trajectory_; - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Last Traj's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds() << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_)); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get()); + resp->trajectory_plan = last_trajectory_; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Last Traj's target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds() << ", and scheduled: " << std::to_string(last_successful_scheduled_entry_time_)); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "EDGE CASE: USING LAST TRAJ: " << (int)last_case_.get()); } else { - last_trajectory_ = trajectory; - resp->trajectory_plan = trajectory; - last_case_ = new_case; - last_final_speeds_ = debug_msg_.velocity_profile; - is_last_case_successful_ = is_new_case_successful; - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING NEW: Target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); - if (is_new_case_successful) - { - last_successful_ending_downtrack_ = GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist); // if algorithm was successful, this is traffic_light_downtrack - last_successful_scheduled_entry_time_ = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds(); // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_)); - } - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING NEW CASE!!! : " << (int)last_case_.get()); + last_trajectory_ = trajectory; + resp->trajectory_plan = trajectory; + last_case_ = new_case; + last_final_speeds_ = debug_msg_.velocity_profile; + is_last_case_successful_ = is_new_case_successful; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING NEW: Target time: " << rclcpp::Time(last_trajectory_.trajectory_points.back().target_time).seconds() << ", and stamp:" << rclcpp::Time(req->header.stamp).seconds()); + if (is_new_case_successful) + { + last_successful_ending_downtrack_ = GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_dist); // if algorithm was successful, this is traffic_light_downtrack + last_successful_scheduled_entry_time_ = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver_plan.front(), end_time)).seconds(); // if algorithm was successful, this is also scheduled entry time (ET in TSMO UC2 Algo) + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "last_successful_ending_downtrack_:" << last_successful_ending_downtrack_ << ", last_successful_scheduled_entry_time_: " << std::to_string(last_successful_scheduled_entry_time_)); + } + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "USING NEW CASE!!! : " << (int)last_case_.get()); } RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Debug: new case:" << (int) new_case << ", is_new_case_successful: " << is_new_case_successful); @@ -217,7 +217,7 @@ namespace light_controlled_intersection_tactical_plugin { if (points_and_target_speeds.empty()) { - throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile..."); + throw std::invalid_argument("Point and target speed list is empty! Unable to apply case one speed profile..."); } // Checking route geometry start against start_dist and adjust profile @@ -243,10 +243,10 @@ namespace light_controlled_intersection_tactical_plugin if (planning_downtrack_start < start_dist) { - //Account for the buffer distance that is technically not part of this maneuver - - total_dist_planned = planning_downtrack_start - start_dist; - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); + //Account for the buffer distance that is technically not part of this maneuver + + total_dist_planned = planning_downtrack_start - start_dist; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "buffered section is present. Adjusted total_dist_planned to: " << total_dist_planned); } double prev_speed = starting_speed; @@ -254,48 +254,48 @@ namespace light_controlled_intersection_tactical_plugin for(auto& p : points_and_target_speeds) { - double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point); - total_dist_planned += delta_d; + double delta_d = lanelet::geometry::distance2d(prev_point.point, p.point); + total_dist_planned += delta_d; - //Apply the speed from algorithm at dist covered - //Kinematic: v_f = sqrt(v_o^2 + 2*a*d) - double speed_i; - if (total_dist_planned <= epsilon_) - { - //Keep target speed same for buffer distance portion - speed_i = starting_speed; - } - else if(total_dist_planned <= dist1 + epsilon_){ - //First segment - speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.a1_ * total_dist_planned); - } - else if(total_dist_planned > dist1 && total_dist_planned <= dist2 + epsilon_){ - //Second segment - speed_i = sqrt(std::max(pow(tsp.v1_, 2) + 2 * tsp.a2_ * (total_dist_planned - dist1), 0.0)); //std::max to ensure negative value is not sqrt - } - else if (total_dist_planned > dist2 && total_dist_planned <= dist3 + epsilon_) - { - //Third segment - speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt - } - else - { - //buffer points that will be cut - speed_i = prev_speed; - } + //Apply the speed from algorithm at dist covered + //Kinematic: v_f = sqrt(v_o^2 + 2*a*d) + double speed_i; + if (total_dist_planned <= epsilon_) + { + //Keep target speed same for buffer distance portion + speed_i = starting_speed; + } + else if(total_dist_planned <= dist1 + epsilon_){ + //First segment + speed_i = sqrt(pow(starting_speed, 2) + 2 * tsp.a1_ * total_dist_planned); + } + else if(total_dist_planned > dist1 && total_dist_planned <= dist2 + epsilon_){ + //Second segment + speed_i = sqrt(std::max(pow(tsp.v1_, 2) + 2 * tsp.a2_ * (total_dist_planned - dist1), 0.0)); //std::max to ensure negative value is not sqrt + } + else if (total_dist_planned > dist2 && total_dist_planned <= dist3 + epsilon_) + { + //Third segment + speed_i = sqrt(std::max(pow(tsp.v2_, 2) + 2 * tsp.a3_ * (total_dist_planned - dist2), 0.0)); //std::max to ensure negative value is not sqrt + } + else + { + //buffer points that will be cut + speed_i = prev_speed; + } - if (isnan(speed_i)) - { - speed_i = std::max(config_.minimum_speed, algo_min_speed); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Detected nan number from equations. Set to " << speed_i); - } + if (isnan(speed_i)) + { + speed_i = std::max(config_.minimum_speed, algo_min_speed); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Detected nan number from equations. Set to " << speed_i); + } - p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed}); - p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned); + p.speed = std::max({speed_i, config_.minimum_speed, algo_min_speed}); + p.speed = std::min({p.speed, speed_limit_, algo_max_speed}); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Applied speed: " << p.speed << ", at dist: " << total_dist_planned); - prev_point = p; - prev_speed = p.speed; + prev_point = p; + prev_speed = p.speed; } } @@ -303,7 +303,7 @@ namespace light_controlled_intersection_tactical_plugin { if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){ - throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters."); + throw std::invalid_argument("There must be 9 float_valued_meta_data and 2 int_valued_meta_data to apply algorithm's parameters."); } TrajectoryParams tsp; @@ -336,11 +336,11 @@ namespace light_controlled_intersection_tactical_plugin lanelet::Optional traffic_rules = wm->getTrafficRules(); if (traffic_rules) { - return (*traffic_rules)->speedLimit(llt).speedLimit.value(); + return (*traffic_rules)->speedLimit(llt).speedLimit.value(); } else { - throw std::invalid_argument("Valid traffic rules object could not be built"); + throw std::invalid_argument("Valid traffic rules object could not be built"); } } @@ -356,26 +356,26 @@ namespace light_controlled_intersection_tactical_plugin RCLCPP_DEBUG_STREAM(logger_->get_logger(), "VehDowntrack: "<get_logger(), "Used downtrack: " << starting_downtrack); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Used downtrack: " << starting_downtrack); - // check if required parameter from strategic planner is present - if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).empty()) - { - throw std::invalid_argument("No time_to_schedule_entry is provided in float_valued_meta_data"); - } + // check if required parameter from strategic planner is present + if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).empty()) + { + throw std::invalid_argument("No time_to_schedule_entry is provided in float_valued_meta_data"); + } - //overwrite maneuver type to use lane follow library function - carma_planning_msgs::msg::Maneuver temp_maneuver = maneuver; - temp_maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Creating Lane Follow Geometry"); - std::vector lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); - - break; // expected to receive only one maneuver to plan + //overwrite maneuver type to use lane follow library function + carma_planning_msgs::msg::Maneuver temp_maneuver = maneuver; + temp_maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Creating Lane Follow Geometry"); + std::vector lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); + points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + + break; // expected to receive only one maneuver to plan } //Add buffer ending to lane follow points at the end of maneuver(s) end dist From 98e230e38c6b2f33f1a4552f4a2db02cee006966 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Wed, 17 Aug 2022 08:23:39 -0700 Subject: [PATCH 071/165] fix indentation --- ...controlled_intersection_tactical_plugin.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 4a864e513d..6d7e590f69 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -33,7 +33,7 @@ namespace light_controlled_intersection_tactical_plugin if(req->maneuver_index_to_plan >= req->maneuver_plan.maneuvers.size()) { - throw std::invalid_argument( + throw std::invalid_argument( "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); } @@ -66,19 +66,19 @@ namespace light_controlled_intersection_tactical_plugin if (current_lanelets.empty()) { - RCLCPP_ERROR_STREAM(logger_->get_logger(), "Given vehicle position is not on the road! Returning..."); - return; + RCLCPP_ERROR_STREAM(logger_->get_logger(), "Given vehicle position is not on the road! Returning..."); + return; } // get the lanelet that is on the route in case overlapping ones found for (auto llt : current_lanelets) { - auto route = wm_->getRoute()->shortestPath(); - if (std::find(route.begin(), route.end(), llt) != route.end()) - { - current_lanelet = llt; - break; - } + auto route = wm_->getRoute()->shortestPath(); + if (std::find(route.begin(), route.end(), llt) != route.end()) + { + current_lanelet = llt; + break; + } } RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Current_lanelet: " << current_lanelet.id()); From 00b389f0d4e51eef405f48fb8cf08ae05768c18d Mon Sep 17 00:00:00 2001 From: JonSmet <34752540+JonSmet@users.noreply.github.com> Date: Wed, 17 Aug 2022 12:12:53 -0400 Subject: [PATCH 072/165] Fix web ui launch (#1892) --- carma/CMakeLists.txt | 2 +- carma/launch/ui.launch.py | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/carma/CMakeLists.txt b/carma/CMakeLists.txt index 3df3e7fafa..ea905a710e 100644 --- a/carma/CMakeLists.txt +++ b/carma/CMakeLists.txt @@ -60,7 +60,7 @@ else() # ROS2 ament_auto_package( - INSTALL_TO_SHARE config launch routes scripts rviz + INSTALL_TO_SHARE config launch routes scripts rviz ui ) endif() diff --git a/carma/launch/ui.launch.py b/carma/launch/ui.launch.py index 71fe357bfb..6f6132f15e 100644 --- a/carma/launch/ui.launch.py +++ b/carma/launch/ui.launch.py @@ -62,8 +62,8 @@ def generate_launch_description(): ("set_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/set_active_route"]), ("start_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/start_active_route"]), ("get_available_routes", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_available_routes"]), - ("route_state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_state]"), - ("route_event", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_event]"), + ("route_state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_state"]), + ("route_event", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_event"]), ("route", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route"]), ("get_system_version", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_system_version"]), ("state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state"]), @@ -93,8 +93,9 @@ def generate_launch_description(): ("can/speed", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/speed"]), ("can/acc_engaged", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/acc_engaged"]) ], - parameters=[ carma_param_file - ] + parameters=[ + carma_param_file + ] ) ]) From dc17aaef57dae1632fa7b0f319e5a92731f6c194 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Thu, 18 Aug 2022 06:24:00 -0700 Subject: [PATCH 073/165] Move lci tactical plugin to plugins.launch.py --- carma/launch/guidance.launch.py | 25 ------------------------- carma/launch/plugins.launch.py | 26 +++++++++++++++++++++++++- 2 files changed, 25 insertions(+), 26 deletions(-) diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index 865bd0d8da..6308da2974 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -76,15 +76,9 @@ def generate_launch_description(): port_drayage_plugin_param_file = os.path.join( get_package_share_directory('port_drayage_plugin'), 'config/parameters.yaml') - inlanecruising_plugin_file_path = os.path.join( - get_package_share_directory('inlanecruising_plugin'), 'config/parameters.yaml') - trajectory_visualizer_param_file = os.path.join( get_package_share_directory('trajectory_visualizer'), 'config/parameters.yaml') - light_controlled_intersection_tactical_plugin_param_file = os.path.join( - get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') - env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') subsystem_controller_param_file = LaunchConfiguration('subsystem_controller_param_file') @@ -250,25 +244,6 @@ def generate_launch_description(): parameters=[ trajectory_visualizer_param_file ] - ), - ComposableNode( - package='light_controlled_intersection_tactical_plugin', - plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', - name='light_controlled_intersection_tactical_plugin', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('light_controlled_intersection_tactical_plugin', env_log_levels) } - ], - 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" ] ), - ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ) - ], - parameters=[ - vehicle_config_param_file, - light_controlled_intersection_tactical_plugin_param_file - ] ) ] ) diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 10e23cb0d5..b155bf2f28 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -51,7 +51,10 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + + light_controlled_intersection_tactical_plugin_param_file = os.path.join( + get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') @@ -126,6 +129,27 @@ def generate_launch_description(): route_following_plugin_file_path, vehicle_config_param_file ] + ), + ComposableNode( + package='light_controlled_intersection_tactical_plugin', + plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', + name='light_controlled_intersection_tactical_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('light_controlled_intersection_tactical_plugin', env_log_levels) } + ], + 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" ] ), + ("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" ] ) + ], + parameters=[ + vehicle_config_param_file, + light_controlled_intersection_tactical_plugin_param_file + ] ) ] ) From eb7d410dbd75b57ca51f4295b0024a8895ded4ae Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Thu, 18 Aug 2022 06:24:44 -0700 Subject: [PATCH 074/165] Add lci tactical plugin to ROS2 guidance plugins --- subsystem_controllers/config/guidance_controller_config.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 5eeac64353..9f04a2f821 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -59,3 +59,4 @@ # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin + - /guidance/plugins/light_controlled_intersection_tactical_plugin From e7fe48309097169c29d811feb2456bbfce310e44 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Mon, 22 Aug 2022 11:10:57 -0400 Subject: [PATCH 075/165] Feature/platooning strategic ros2 (#1894) Description This PR ports the platooning strategic ihp plugin to ROS2 as well as fixes a number of bugs associated with recent ports. Changes: platoon_strategic_ihp ported to ROS2 platoon_strategic removed as it has been fully replaced by platoon_strategic_ihp Updates some platooning unit tests to compile and pass. They had been very out of date during ihp2 testing. There are still 7 failing tests which need to be updated by someone with more knowledge of the state machine. But these tests are also failing in ROS1. ui.launch.py corrected to use the launch file from rosbrige_websocket. It was previously trying to load a python node as a component which is impossible. Removes the UI parameters file which didn't have a clear path to loading and has never been modified in carma2 or 3 so defaults should be plenty sufficient. Corrects a bug in the stop_and_wait_plugin where the wrong library was specified as the component making the class unloadable Corrects a bug in route_following_plugin where a decimal parameter was marked as an integer This PR should be merged simultaneously with usdot-fhwa-stol/carma-config#207 and usdot-fhwa-stol/carma-web-ui#148 Related Issue Resolves #1895 Resolves #1896 Resolves #1897 Resolves #1898 Resolves #1899 --- carma/launch/carma_src.launch.py | 4 +- carma/launch/guidance.launch | 16 - carma/launch/plugins.launch.py | 53 +- carma/launch/ui.launch.py | 118 +- carma/ui/config/CommandAPIParams.yaml | 9 - docker/checkout.bash | 2 +- platooning_strategic/CMakeLists.txt | 115 - platooning_strategic/config/parameters.yaml | 19 - platooning_strategic/include/platoon_config.h | 82 - .../include/platoon_manager.h | 287 - .../include/platoon_strategic.h | 605 - .../include/platoon_strategic_plugin_node.h | 109 - .../launch/platoon_strategic.launch | 27 - platooning_strategic/package.xml | 21 - platooning_strategic/src/main.cpp | 26 - platooning_strategic/src/platoon_manager.cpp | 433 - .../src/platoon_strategic.cpp | 1603 - platooning_strategic/test/main_test.cpp | 27 - .../test/mobility_messages.cpp | 277 - .../test/test_platoon_manager.cpp | 616 - .../test/town01_vector_map_lane_change.osm | 55348 ---------------- platooning_strategic_IHP/CMakeLists.txt | 204 +- platooning_strategic_IHP/README.md | 9 + .../platoon_config_ihp.h | 7 +- .../platoon_manager_ihp.h | 31 +- .../platoon_strategic_ihp.h | 221 +- .../platoon_strategic_plugin_node_ihp.h | 113 + .../platoon_strategic_plugin_node_ihp.h | 125 - .../launch/platoon_strategic_ihp.launch | 29 - .../launch/platoon_strategic_ihp_launch.py | 68 + platooning_strategic_IHP/package.xml | 54 +- .../src/{main_ihp.cpp => main.cpp} | 30 +- .../src/platoon_manager_ihp.cpp | 140 +- .../src/platoon_strategic_ihp.cpp | 1238 +- .../src/platoon_strategic_ihp_node.cpp | 241 + platooning_strategic_IHP/test/main_test.cpp | 57 +- .../test/mobility_messages.cpp | 88 +- .../test/test_platoon_manager.cpp | 117 +- .../test/test_platoon_manager_front_join.cpp | 182 +- route_following_plugin/config/parameters.yaml | 2 +- stop_and_wait_plugin/CMakeLists.txt | 2 +- .../guidance_controller.cpp | 4 + .../guidance_controller/plugin_manager.cpp | 6 +- 43 files changed, 1726 insertions(+), 61039 deletions(-) delete mode 100644 carma/ui/config/CommandAPIParams.yaml delete mode 100644 platooning_strategic/CMakeLists.txt delete mode 100644 platooning_strategic/config/parameters.yaml delete mode 100644 platooning_strategic/include/platoon_config.h delete mode 100644 platooning_strategic/include/platoon_manager.h delete mode 100644 platooning_strategic/include/platoon_strategic.h delete mode 100644 platooning_strategic/include/platoon_strategic_plugin_node.h delete mode 100644 platooning_strategic/launch/platoon_strategic.launch delete mode 100644 platooning_strategic/package.xml delete mode 100644 platooning_strategic/src/main.cpp delete mode 100644 platooning_strategic/src/platoon_manager.cpp delete mode 100644 platooning_strategic/src/platoon_strategic.cpp delete mode 100644 platooning_strategic/test/main_test.cpp delete mode 100644 platooning_strategic/test/mobility_messages.cpp delete mode 100644 platooning_strategic/test/test_platoon_manager.cpp delete mode 100755 platooning_strategic/test/town01_vector_map_lane_change.osm create mode 100644 platooning_strategic_IHP/README.md rename platooning_strategic_IHP/include/{ => platoon_strategic_ihp}/platoon_config_ihp.h (98%) rename platooning_strategic_IHP/include/{ => platoon_strategic_ihp}/platoon_manager_ihp.h (96%) rename platooning_strategic_IHP/include/{ => platoon_strategic_ihp}/platoon_strategic_ihp.h (79%) create mode 100644 platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_plugin_node_ihp.h delete mode 100644 platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h delete mode 100644 platooning_strategic_IHP/launch/platoon_strategic_ihp.launch create mode 100644 platooning_strategic_IHP/launch/platoon_strategic_ihp_launch.py rename platooning_strategic_IHP/src/{main_ihp.cpp => main.cpp} (56%) create mode 100644 platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp diff --git a/carma/launch/carma_src.launch.py b/carma/launch/carma_src.launch.py index 5341ea9b24..ac2b135246 100644 --- a/carma/launch/carma_src.launch.py +++ b/carma/launch/carma_src.launch.py @@ -239,6 +239,6 @@ def generate_launch_description(): localization_group, v2x_group, guidance_group, - system_controller, - ui_group + ui_group, + system_controller ]) diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index bf391b44f7..c32ffbc52d 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -156,22 +156,6 @@ - - - - - - - - - - - diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 10e23cb0d5..3cefa1b9b6 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -51,13 +51,16 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + + platoon_strategic_ihp_param_file = os.path.join( + get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') carma_plugins_container = ComposableNodeContainer( package='carma_ros2_utils', - name='carma_guidance_plugins_container', + name='carma_guidance_core_plugins_container', executable='carma_component_container_mt', namespace=GetCurrentNamespace(), composable_node_descriptions=[ @@ -130,6 +133,50 @@ def generate_launch_description(): ] ) + platooning_plugins_container = ComposableNodeContainer( + package='carma_ros2_utils', + name='platooning_plugins_container', + executable='carma_component_container_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='platoon_strategic_ihp', + plugin='platoon_strategic_ihp::Node', + name='platoon_strategic_ihp_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('platoon_strategic_ihp', env_log_levels) } + ], + 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" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("outgoing_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_response" ] ), + ("outgoing_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_request" ] ), + ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), + ("incoming_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ), + ("incoming_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_response" ] ), + ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), + ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ), + ("twist_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/twist_raw" ] ), + ("platoon_info", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/platoon_info" ] ), + ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ), + ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ), + ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ], + parameters=[ + platoon_strategic_ihp_param_file, + vehicle_config_param_file + ] + ), + ] + ) + + + return LaunchDescription([ - carma_plugins_container + carma_plugins_container, + platooning_plugins_container ]) diff --git a/carma/launch/ui.launch.py b/carma/launch/ui.launch.py index 6f6132f15e..afec51489c 100644 --- a/carma/launch/ui.launch.py +++ b/carma/launch/ui.launch.py @@ -13,18 +13,17 @@ # limitations under the License. from ament_index_python import get_package_share_directory -from launch.actions import Shutdown from launch import LaunchDescription -from launch_ros.actions import Node -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode from launch.substitutions import EnvironmentVariable -from carma_ros2_utils.launch.get_log_level import GetLogLevel -from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace from launch.substitutions import LaunchConfiguration from launch.actions import DeclareLaunchArgument -import os +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.substitutions import EnvironmentVariable +from launch.actions import GroupAction +from launch_ros.actions.set_remap import SetRemap +import os def generate_launch_description(): """ @@ -38,70 +37,55 @@ def generate_launch_description(): default_value = "9090", description = "The default port for rosbridge is 9090" ) - - carma_param_file = os.path.join( - get_package_share_directory('carma'), 'ui/config/CommandAPIParams.yaml') - ui_container = ComposableNodeContainer( - package='carma_ros2_utils', # rclcpp_components - name='ui_container', - executable='lifecycle_component_wrapper_mt', - namespace=GetCurrentNamespace(), - composable_node_descriptions=[ - ComposableNode( - package='rosbridge_server', - plugin='rosbridge_websocket', - name='rosbridge_server', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('rosbridge_server', env_log_levels) }, - {'is_lifecycle_node': True} # Flag to allow lifecycle node loading in lifecycle wrapper - ], - remappings=[ - ("get_available_routes",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''),"/get_available_routes"]), - ("set_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/set_active_route"]), - ("start_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/start_active_route"]), - ("get_available_routes", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_available_routes"]), - ("route_state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_state"]), - ("route_event", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_event"]), - ("route", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route"]), - ("get_system_version", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_system_version"]), - ("state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state"]), - ("ui_platoon_vehicle_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ui_platoon_vehicle_info"]), - ("plugins/available_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/available_plugins"]), - ("plugins/get_registered_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/get_registered_plugins"]), - ("plugins/activate_plugin", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/activate_plugin"]), - ("get_available_routes", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_available_routes"]), - ("set_guidance_active", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/set_guidance_active"]), - ("plugins/controlling_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/controlling_plugins"]), - ("traffic_signal_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/traffic_signal_info"]), - ("platooning_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/platooning_info"]), - ("traffic_signal_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/traffic_signal_info"]), - ("system_alert", "/system_alert"), - - ("bsm", [EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm"]), - - ("nav_sat_fix", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/gnss/nav_sat_fix"]), - ("velocity", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist"]), - ("driver_discovery", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/driver_discovery"]), - ("get_drivers_with_capabilities", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/get_drivers_with_capabilities"]), - ("controller/robotic_status", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/robot_status"]), - ("controller/vehicle_cmd" , [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/vehicle_cmd"]), - ("comms/outbound_binary_msg",[EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/comms/outbound_binary_msg"]), - ("comms/inbound_binary_msg", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/comms/inbound_binary_msg"]), - ("can/engine_speed", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/engine_speed"]), - ("can/speed", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/speed"]), - ("can/acc_engaged", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/acc_engaged"]) - ], - parameters=[ - carma_param_file - ] - ) - ]) + rosbridge_server_group = GroupAction( + actions=[ + SetRemap("get_available_routes",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''),"/get_available_routes"]), + SetRemap("set_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/set_active_route"]), + SetRemap("start_active_route",[EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/start_active_route"]), + SetRemap("get_available_routes", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_available_routes"]), + SetRemap("route_state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_state"]), + SetRemap("route_event", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route_event"]), + SetRemap("route", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route"]), + SetRemap("get_system_version", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_system_version"]), + SetRemap("state", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/state"]), + SetRemap("ui_platoon_vehicle_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ui_platoon_vehicle_info"]), + SetRemap("plugins/available_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/available_plugins"]), + SetRemap("plugins/get_registered_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/get_registered_plugins"]), + SetRemap("plugins/activate_plugin", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/activate_plugin"]), + SetRemap("get_available_routes", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/get_available_routes"]), + SetRemap("set_guidance_active", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/set_guidance_active"]), + SetRemap("plugins/controlling_plugins", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/controlling_plugins"]), + SetRemap("traffic_signal_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/traffic_signal_info"]), + SetRemap("platooning_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/platooning_info"]), + SetRemap("traffic_signal_info", [EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/traffic_signal_info"]), + SetRemap("system_alert", "/system_alert"), + SetRemap("bsm", [EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_bsm"]), + SetRemap("nav_sat_fix", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/gnss/nav_sat_fix"]), + SetRemap("velocity", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist"]), + SetRemap("driver_discovery", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/driver_discovery"]), + SetRemap("get_drivers_with_capabilities", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/get_drivers_with_capabilities"]), + SetRemap("controller/robotic_status", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/robot_status"]), + SetRemap("controller/vehicle_cmd" , [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/controller/vehicle_cmd"]), + SetRemap("comms/outbound_binary_msg",[EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/comms/outbound_binary_msg"]), + SetRemap("comms/inbound_binary_msg", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/comms/inbound_binary_msg"]), + SetRemap("can/engine_speed", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/engine_speed"]), + SetRemap("can/speed", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/speed"]), + SetRemap("can/acc_engaged", [EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/can/acc_engaged"]), + IncludeLaunchDescription( + AnyLaunchDescriptionSource( + [ get_package_share_directory('rosbridge_server'), '/launch/rosbridge_websocket_launch.xml'] + ), + launch_arguments = { + 'port' : port, + }.items() + ), + ] + ) return LaunchDescription([ declare_port, - ui_container + rosbridge_server_group ]) diff --git a/carma/ui/config/CommandAPIParams.yaml b/carma/ui/config/CommandAPIParams.yaml deleted file mode 100644 index 9932b0e7d0..0000000000 --- a/carma/ui/config/CommandAPIParams.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# CommandAPIParams.yaml -# Defines the ros parameters which are required by the CommandAPI - -# String: Host vehicle may have different instructions to show to the Driver before engaging the guidance system. -host_instructions: 'This is the sample host vehicle instructions' - -# Integer: Interval in milliseconds to refresh the web page, usually to update a display page. -# Units: Milliseconds -page_refresh_interval: 100 \ No newline at end of file diff --git a/docker/checkout.bash b/docker/checkout.bash index c7e4fc4e8a..464faf0157 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -67,4 +67,4 @@ echo "" > COLCON_IGNORE cd ../ #rosbridge_suite is a ROS meta-package including all the rosbridge packages. -git clone https://github.com/usdot-fhwa-stol/rosbridge_suite +git clone https://github.com/usdot-fhwa-stol/rosbridge_suite --branch ros2 diff --git a/platooning_strategic/CMakeLists.txt b/platooning_strategic/CMakeLists.txt deleted file mode 100644 index ab1ef6a4a1..0000000000 --- a/platooning_strategic/CMakeLists.txt +++ /dev/null @@ -1,115 +0,0 @@ -# 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. - -cmake_minimum_required(VERSION 2.8.3) -project(platoon_strategic) - -find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) -carma_package() - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - carma_utils - cav_msgs - cav_srvs - roscpp - std_msgs - carma_wm - lanelet2_extension - tf - tf2 - tf2_ros - tf2_geometry_msgs -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED) - -################################### -## catkin specific configuration ## -################################### - - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS carma_utils cav_msgs roscpp std_msgs carma_wm cav_srvs lanelet2_extension -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - - -add_executable( ${PROJECT_NAME} - src/platoon_strategic.cpp - src/platoon_manager.cpp - src/main.cpp) - - -add_library(platoon_strategic_plugin_lib - src/platoon_strategic.cpp - src/platoon_manager.cpp) - -add_dependencies(platoon_strategic_plugin_lib ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(platoon_strategic_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} platoon_strategic_plugin_lib) - - -############# -## Install ## -############# - - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -## Install C++ -install(TARGETS ${PROJECT_NAME} platoon_strategic_plugin_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - - -############# -## Testing ## -############# - -catkin_add_gmock(${PROJECT_NAME}-test - test/test_platoon_manager.cpp - test/mobility_messages.cpp - test/main_test.cpp) - -if(TARGET ${PROJECT_NAME}-test) - target_link_libraries(${PROJECT_NAME}-test platoon_strategic_plugin_lib ${catkin_LIBRARIES}) -endif() diff --git a/platooning_strategic/config/parameters.yaml b/platooning_strategic/config/parameters.yaml deleted file mode 100644 index 7cd1b303ea..0000000000 --- a/platooning_strategic/config/parameters.yaml +++ /dev/null @@ -1,19 +0,0 @@ -maxPlatoonSize: 10 -algorithmType: 0 -mvr_duration: 15 -timeHeadway: 2.5 -standStillHeadway: 12.0 -maxAllowedJoinTimeGap: 15.0 -maxAllowedJoinGap: 90.0 -desiredJoinTimeGap: 4.0 -desiredJoinGap: 30.0 -waitingStateTimeout: 25.0 -cmdSpeedMaxAdjustment: 10.0 -lowerBoundary: 1.6 -upperBoundary: 1.7 -maxSpacing: 4.0 -minSpacing: 3.9 -minGap: 22.0 -maxGap: 32.0 -maxCrosstrackError: 0.8 -vehicleLength: 5.0 diff --git a/platooning_strategic/include/platoon_config.h b/platooning_strategic/include/platoon_config.h deleted file mode 100644 index c64366cdc0..0000000000 --- a/platooning_strategic/include/platoon_config.h +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once - -/* - * Copyright (C) 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 - -/** - * \brief Stuct containing the algorithm configuration values for the PlatoonStrategicPlugin - */ -struct PlatoonPluginConfig -{ - // following parameters are for general platooning plugin - double vehicleLength = 5.0; // m - int maxPlatoonSize = 10; // 1 - int algorithmType = 0; // N/A - int statusMessageInterval = 100; // ms - int infoMessageInterval = 200; // ms - double mvr_duration = 15; // s - double epislon = 0.001; // m/s - - // following parameters are for platoon forming and operation - double timeHeadway = 2.0; // s - double standStillHeadway = 12.0; // m - double maxAllowedJoinTimeGap = 15.0; // s - double maxAllowedJoinGap = 90.0; // m - double desiredJoinTimeGap = 4.0; // s - double desiredJoinGap = 30.0; // m - double waitingStateTimeout = 25.0; // s - double cmdSpeedMaxAdjustment = 10.0; // m/s - - // following parameters are mainly for APF leader selection - double lowerBoundary = 1.6; // s - double upperBoundary = 1.7; // s - double maxSpacing = 4.0; // s - double minSpacing = 3.9; // s - double minGap = 22.0; // m - double maxGap = 32.0; // m - double maxCrosstrackError = 2.0; // m - - std::string vehicleID = "default_id"; - - - friend std::ostream& operator<<(std::ostream& output, const PlatoonPluginConfig& c) - { - output << "PlatoonPluginConfig { " << std::endl - << "maxPlatoonSize: " << c.maxPlatoonSize << std::endl - << "algorithmType: " << c.algorithmType << std::endl - << "statusMessageInterval: " << c.statusMessageInterval << std::endl - << "infoMessageInterval: " << c.infoMessageInterval << std::endl - << "timeHeadway: " << c.timeHeadway << std::endl - << "standStillHeadway: " << c.standStillHeadway << std::endl - << "maxAllowedJoinTimeGap: " << c.maxAllowedJoinTimeGap << std::endl - << "desiredJoinTimeGap: " << c.desiredJoinTimeGap << std::endl - << "desiredJoinGap: " << c.desiredJoinGap << std::endl - << "waitingStateTimeout: " << c.waitingStateTimeout << std::endl - << "cmdSpeedMaxAdjustment: " << c.cmdSpeedMaxAdjustment << std::endl - << "lowerBoundary: " << c.lowerBoundary << std::endl - << "upperBoundary: " << c.upperBoundary << std::endl - << "maxSpacing: " << c.maxSpacing << std::endl - << "minSpacing: " << c.minSpacing << std::endl - << "kpminGapPID: " << c.minGap << std::endl - << "maxGap: " << c.maxGap << std::endl - << "maxCrosstrackError: " << c.maxCrosstrackError << std::endl - << "vehicleID: " << c.vehicleID << std::endl - << "}" << std::endl; - return output; - } -}; diff --git a/platooning_strategic/include/platoon_manager.h b/platooning_strategic/include/platoon_manager.h deleted file mode 100644 index 954be46b4e..0000000000 --- a/platooning_strategic/include/platoon_manager.h +++ /dev/null @@ -1,287 +0,0 @@ -#pragma once - -/* - * Copyright (C) 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 -#include -#include -#include -#include -#include -#include - - - - -namespace platoon_strategic -{ - /** - * \brief Struct for a platoon plan - */ - struct PlatoonPlan { - - bool valid; - long planStartTime; - std::string planId; - std::string peerId; - PlatoonPlan():valid(false), planStartTime(0), planId(""), peerId("") {} ; - PlatoonPlan(bool valid, long planStartTime, std::string planId, std::string peerId): - valid(valid), planStartTime(planStartTime), planId(planId), peerId(peerId) {} - }; - - /** - * \brief A response to an MobilityRequest message. - * ACK - indicates that the plugin accepts the MobilityRequest and will handle making any adjustments needed to avoid a conflict - * NACK - indicates that the plugin rejects the MobilityRequest and would suggest the other vehicle replan - * NO_RESPONSE - indicates that the plugin is indifferent but sees no conflict - */ - enum MobilityRequestResponse { - ACK, - NACK, - NO_RESPONSE - }; - - /** - * \brief Platoon States - */ - enum PlatoonState{ - STANDBY, - LEADERWAITING, - LEADER, - CANDIDATEFOLLOWER, - FOLLOWER - }; - - /** - * \brief Platoon States - */ - struct PlatoonMember{ - // Static ID is permanent ID for each vehicle - std::string staticId; - // Current BSM Id for each CAV - std::string bsmId; - // Vehicle real time command speed in m/s - double commandSpeed; - // Actual vehicle speed in m/s - double vehicleSpeed; - // Vehicle current down track distance on the current route in m - double vehiclePosition; - // The local time stamp when the host vehicle update any informations of this member - long timestamp; - PlatoonMember(): staticId(""), bsmId(""), commandSpeed(0.0), vehicleSpeed(0.0), vehiclePosition(0.0), timestamp(0) {} - PlatoonMember(std::string staticId, std::string bsmId, double commandSpeed, double vehicleSpeed, double vehiclePosition, long timestamp): staticId(staticId), - bsmId(bsmId), commandSpeed(commandSpeed), vehicleSpeed(vehicleSpeed), vehiclePosition(vehiclePosition), timestamp(timestamp) {} - }; - - - - class PlatoonManager - { - public: - - /** - * \brief Default constructor - */ - PlatoonManager(); - - // Platoon List (initialized empty) - std::vector platoon{}; - - - - // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; - - // Current vehicle downtrack - double current_downtrack_distance_ = 0; - - /** - * \brief Update platoon members information - * - * \param senderId static id of the broadcasting vehicle - * \param platoonId platoon id - * \param senderBsmId bsm id of the broadcasting vehicle - * \param params strategy parameters - * \param Dtd downtrack distance - */ - void memberUpdates(const std::string& senderId,const std::string& platoonId,const std::string& senderBsmId,const std::string& params, const double& DtD); - - /** - * Given any valid platooning mobility STATUS operation parameters and sender staticId, - * in leader state this method will add/updates the information of platoon member if it is using - * the same platoon ID, in follower state this method will updates the vehicle information who - * is in front of the subject vehicle or update platoon id if the leader is join another platoon - * \param senderId sender ID for the current info - * \param platoonId sender platoon id - * \param senderBsmId sender BSM ID - * \param params strategy params from STATUS message in the format of "CMDSPEED:xx,DOWNTRACK:xx,SPEED:xx" - **/ - void updatesOrAddMemberInfo(std::string senderId, std::string senderBsmId, double cmdSpeed, double dtDistance, double curSpeed); - - /** - * \brief Returns total size of the platoon - */ - int getTotalPlatooningSize() const; - - - /** - * \brief Returns leader of the platoon - */ - PlatoonMember getLeader(); - - /** - * This is the implementation of all predecessor following (APF) algorithm for leader - * selection in a platoon. This function will recognize who is acting as the current leader - * of the subject vehicle. The current leader of the subject vehicle will be any ONE of - * the vehicles in front of it. Having a vehicle further downstream function as the leader - * is more efficient and more stable; however, having a vehicle closer to the subject vehicle - * function as the leader is safer. For this reason, the subject vehicle will monitor - * all time headways between every single set of consecutive vehicles starting from itself - * to the leader. If the time headways are within some safe thresholds then vehicles further - * downstream may function as the leader. Otherwise, for the sake of safety, vehicles closer - * to the subject vehicle, potentially even the predecessor, will function as the leader. - * @return the index of the leader in the platoon list - */ - - int allPredecessorFollowing(); - - /** - * \brief Update status when state change from Follower to Leader - */ - void changeFromFollowerToLeader(); - - /** - * \brief Update status when state change from Leader to Follower - * - * \param newPlatoonId platoon id of the leader - */ - void changeFromLeaderToFollower(std::string newPlatoonId); - - /** - * \brief Get number of vehicles in front of host vehicle in platoon - */ - int getNumberOfVehicleInFront(); - - /** - * \brief Returns Length of the platoon - */ - double getCurrentPlatoonLength(); - - /** - * \brief Returns downtrack distance of the rear vehicle in platoon - */ - double getPlatoonRearDowntrackDistance(); - - /** - * \brief Returns distance from start of the route - */ - double getDistanceFromRouteStart() const; - - /** - * \brief Returns distance to the front vehicle - */ - double getDistanceToFrontVehicle(); - - /** - * \brief Returns current speed - */ - double getCurrentSpeed() const; - - /** - * \brief Returns command speed - */ - double getCommandSpeed(); - - /** - * \brief Returns current downtrack distance - */ - double getCurrentDowntrackDistance() const; - - // Member variables - int platoonSize = 2; - std::string leaderID = "default_leader_id"; - std::string currentPlatoonID = "default_test_id"; - bool isFollower = false; - - double current_speed_ = 0; - double command_speed_ = 0; - - // Platoon State - PlatoonState current_platoon_state = PlatoonState::STANDBY; - - // Platooning PLan - PlatoonPlan current_plan; - - std::string targetLeaderId = "default_target_leader_id"; - - std::string HostMobilityId = "default_host_id"; - - - private: - - std::string targetPlatoonId; - std::string OPERATION_INFO_TYPE = "INFO"; - std::string OPERATION_STATUS_TYPE = "STATUS"; - std::string JOIN_AT_REAR_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%"; - std::string MOBILITY_STRATEGY = "Carma/Platooning"; - - - double minGap_ = 22.0; - double maxGap_ = 32.0; - std::string previousFunctionalLeaderID_ = ""; - int previousFunctionalLeaderIndex_ = -1; - - double maxSpacing_ = 4.0; - double minSpacing_ = 3.9; - double lowerBoundary_ = 1.6; - double upperBoundary_ = 1.7 ; - - double vehicleLength_ = 5.0; // m - - double gapWithFront_ = 0.0; - - double downtrack_progress_ = 0; - - - std::string algorithmType_ = "APF_ALGORITHM"; - - bool insufficientGapWithPredecessor(double distanceToFrontVehicle); - - std::vector calculateTimeHeadway(std::vector downtrackDistance, std::vector speed) const; - int determineLeaderBasedOnViolation(std::vector timeHeadways); - - // helper method for APF algorithm - int findLowerBoundaryViolationClosestToTheHostVehicle(std::vector timeHeadways) const; - - // helper method for APF algorithm - int findMaximumSpacingViolationClosestToTheHostVehicle(std::vector timeHeadways) const; - - std::vector getTimeHeadwayFromIndex(std::vector timeHeadways, int start) const; - - - - - - - }; -} \ No newline at end of file diff --git a/platooning_strategic/include/platoon_strategic.h b/platooning_strategic/include/platoon_strategic.h deleted file mode 100644 index 827ccbdba2..0000000000 --- a/platooning_strategic/include/platoon_strategic.h +++ /dev/null @@ -1,605 +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. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "platoon_config.h" -#include -#include -#include -#include -#include -#include -#include -#include - -namespace platoon_strategic -{ - /** - * \brief Macro definition to enable easier access to fields shared across the maneuver types - * \param mvr The maneuver object to invoke the accessors on - * \param property The name of the field to access on the specific maneuver types. Must be shared by all extant maneuver types - * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field - */ - #define GET_MANEUVER_PROPERTY(mvr, property)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\ - throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))))) - - - - using PublishPluginDiscoveryCB = std::function; - using MobilityResponseCB = std::function; - using MobilityRequestCB = std::function; - using MobilityOperationCB = std::function; - using PlatooningInfoCB = std::function; - - class PlatoonStrategicPlugin - { - public: - - /** - * \brief Default constructor for PlatoonStrategicPlugin class - */ - PlatoonStrategicPlugin(); - - /** - * \brief Constructor - * - * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data - * \param config The configuration to be used for this object - * \param plugin_discovery_publisher Callback which will publish the current plugin discovery state - */ - PlatoonStrategicPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher, MobilityResponseCB mobility_response_publisher, - MobilityRequestCB mobility_request_publisher, MobilityOperationCB mobility_operation_publisher, - PlatooningInfoCB platooning_info_publisher); - - - /** - * \brief Callback function for Mobility Operation Message - * - * \param msg Mobility Operation Message - */ - void mob_op_cb(const cav_msgs::MobilityOperation& msg); - - /** - * \brief Callback function for Mobility Request Message - * - * \param msg Mobility Request Message - */ - void mob_req_cb(const cav_msgs::MobilityRequest& msg); - - /** - * \brief Callback function for Mobility Response Message - * - * \param msg Mobility Response Message - */ - void mob_resp_cb(const cav_msgs::MobilityResponse& msg); - - /** - * \brief Function to the process and respond to the mobility request - * - * \param msg Mobility Request Message - * - * \return Mobility response message - */ - MobilityRequestResponse handle_mob_req(const cav_msgs::MobilityRequest& msg); - - /** - * \brief Callback function to the maneuver request - * - * \param req Maneuver service request - * \param resp Maneuver service response - * - * \return Mobility response message - */ - bool plan_maneuver_cb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp); - - /** - * \brief Find lanelet index from path - * - * \param path path - * \param target_id target lanelet id - * - * \return lanelet index - */ - int findLaneletIndexFromPath(int target_id, lanelet::routing::LaneletPath& path); - - /** - * \brief Find lanelet index from path - * - * \param current_dist current downtrack distance - * \param end_dist ending downtrack distance - * \param current_speed current speed - * \param target_speed target speed - * \param lane_id lanelet id - * \param current_time current time in seconds - * - * \return Maneuver message - */ - cav_msgs::Maneuver composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time& current_time); - - /** - * \brief Update maneuver status based on prior plan - * - * \param maneuver maneuver - * \param speed speed - * \param current_progress current progress - * \param lane_id lanelet ud - */ - void updateCurrentStatus(cav_msgs::Maneuver maneuver, double& speed, double& current_progress, int& lane_id); - - /** - * \brief Callback function for current pose - * - * \param msg PoseStamped msg - */ - void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); - - /** - * \brief Compose Platoon information message - * - * \return PlatooningInfo msg - */ - cav_msgs::PlatooningInfo composePlatoonInfoMsg(); - - /** - * \brief Callback for the twist subscriber, which will store latest twist locally - * \param msg Latest twist message - */ - void twist_cb(const geometry_msgs::TwistStampedConstPtr& msg); - - /** - * \brief Callback for the control command - * \param msg Latest twist cmd message - */ - void cmd_cb(const geometry_msgs::TwistStampedConstPtr& msg); - - /** - * \brief Callback for the BSM - * \param msg Latest BSM message - */ - void bsm_cb(const cav_msgs::BSMConstPtr& msg); - - /** - * \brief Callback for the georeference - * \param msg Latest georeference - */ - void georeference_cb(const std_msgs::StringConstPtr& msg); - - /** - * \brief Spin callback function - */ - bool onSpin(); - - // Platoon Manager Object - PlatoonManager pm_; - - /** - * \brief Run Leader State - */ - void run_leader(); - - /** - * \brief Run Leader Waiting State - */ - void run_leader_waiting(); - - /** - * \brief Run Candidate Follower State - */ - void run_candidate_follower(); - - /** - * \brief Run Follower State - */ - void run_follower(); - - /** - * \brief Checks if the current location is on the rightmost lane. - * \param current_location 2d point of current location x and y - */ - void checkForRightMostLane(const lanelet::BasicPoint2d& current_location); - - /** - * \brief Gets the local lane index of a provided location. The local lane index is 0 for rightmost lane, - * 1 for second rightmost, etc. The local lane index considers only the current travel direction. - * \param current_location 2d point of current location x and y. - * \return The local lane index of the provided location. - */ - int getCurrentLaneIndex(const lanelet::BasicPoint2d& current_location); - - /** - * \brief Gets the quantity of lanes in the local lane group of a provided location. This includes the - * quantity of all side-by-side lanelets including the current lanelet. It considers only the current travel direction. - * \param current_location 2d point of current location x and y. - * \return Quantity of lanes in the local lane group of the provided location. - */ - int getCurrentLaneGroupSize(const lanelet::BasicPoint2d& current_location); - - /** - * \brief Compose a lane change maneuver message based on input params - * \param start_dist Start downtrack distance of the current maneuver - * \param end_dist End downtrack distance of the current maneuver - * \param start_speed Start speed of the current maneuver - * \param target_speed Target speed pf the current maneuver, usually it is the lanelet speed limit - * \param starting_lane_id starting lane id of the lane change - * \param ending_lane_id starting lane id of the lane change - * \return A lane keeping maneuver message which is ready to be published - */ - cav_msgs::Maneuver composeLaneChangeManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id) const; - - - // ECEF position of the host vehicle - cav_msgs::LocationECEF pose_ecef_point_; - - // variable to show if the vehicle is on the rightmost lane - bool in_rightmost_lane_ = true; - // variable to show if the vehicle is on a single-lane lane - bool single_lane_road_ = false; - - // Flag to enable/disable platooning plugin - bool platooning_enabled_ = false; - - - - private: - - - PublishPluginDiscoveryCB plugin_discovery_publisher_; - MobilityRequestCB mobility_request_publisher_; - MobilityResponseCB mobility_response_publisher_; - MobilityOperationCB mobility_operation_publisher_; - PlatooningInfoCB platooning_info_publisher_; - - - // wm listener pointer and pointer to the actual wm object - std::shared_ptr wml_; - carma_wm::WorldModelConstPtr wm_; - - // local copy of pose - PlatoonPluginConfig config_; - - // local copy of pose - // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; - - //Internal Variables used in unit tests - // Current vehicle command speed - double cmd_speed_ = 0; - // Current vehicle measured speed - double current_speed_ = 0; - // Current vehicle downtrack distance in route - double current_downtrack_ = 0; - // Current vehicle crosstrack distance in route - double current_crosstrack_ = 0; - // start time of leaderwaiting state - long waitingStartTime = 0; - // start time of candidate follower state - long candidatestateStartTime = 0; - // potential new platood id - std::string potentialNewPlatoonId = ""; - // target platood id - std::string targetPlatoonId = ""; - // Host Mobility ID - std::string HostMobilityId = "hostid"; - // Host BSM ID - std::string host_bsm_id_ = ""; - - // leader_waiting applicant id - std::string lw_applicantId_ = ""; - - // Current local lane index of host vehicle's local lane group (0 is rightmost, 1 is second rightmost, etc.); considers only the current travel direction - int current_lane_index_ = 0; - // Current quantity of lanes in the host vehicle's local lane group (side-by-side lanelets including the current lanelet; considers only the current travel direction) - int current_lane_group_size_ = 0; - // CandidateFollower required local lane index before it can issue PLATOON_FOLLOWER_JOIN MobilityRequest to target leader - int cf_target_lane_index_ = 0; - // Flag to indicate whether CandidateFollower has received JOIN_REQUIREMENTS MobilityOperation message from target leader - bool has_received_join_requirements_ = false; - - // ROS Publishers - ros::Publisher platoon_strategic_plugin_discovery_pub_; - ros::Publisher mob_op_pub_; - ros::Publisher mob_req_pub_; - - // ROS Subscribers - ros::Subscriber pose_sub_; - ros::Subscriber mob_req_sub_; - ros::Subscriber mob_resp_sub_; - ros::Subscriber mob_op_sub_; - - - /** - * \brief Function to determin if a vehicle is in the front of hose vehicle - * - * \param rearVehicleBsmId rear vehicle bsm id - * \param downtrack vehicle downtrack - * - * \return true or false - */ - bool isVehicleRightInFront(std::string rearVehicleBsmId, double downtrack); - - /** - * \brief Function to find speed limit of a lanelet - * - * \param llt inout lanelet - * - * \return speed limit value - */ - double findSpeedLimit(const lanelet::ConstLanelet& llt); - - - /** - * \brief Function to process mobility request in leader state - * - * \param msg incoming mobility request - * - * \return ACK, NACK, or No response - */ - MobilityRequestResponse mob_req_cb_leader(const cav_msgs::MobilityRequest& msg); - - /** - * \brief Function to process mobility request in leader waiting state - * - * \param msg incoming mobility request - * - * \return ACK, NACK, or No response - */ - MobilityRequestResponse mob_req_cb_leaderwaiting(const cav_msgs::MobilityRequest& msg); - - /** - * \brief Function to process mobility request in follower state - * - * \param msg incoming mobility request - * - * \return ACK, NACK, or No response - */ - MobilityRequestResponse mob_req_cb_follower(const cav_msgs::MobilityRequest& msg); - - /** - * \brief Function to process mobility request in candidate follower state - * - * \param msg incoming mobility request - * - * \return ACK, NACK, or No response - */ - MobilityRequestResponse mob_req_cb_candidatefollower(const cav_msgs::MobilityRequest& msg); - - /** - * \brief Function to process mobility request in standby state - * - * \param msg incoming mobility request - * - * \return ACK, NACK, or No response - */ - MobilityRequestResponse mob_req_cb_standby(const cav_msgs::MobilityRequest& msg); - - /** - * \brief Function to process mobility response in leader state - * - * \param msg incoming mobility response - */ - void mob_resp_cb_leader(const cav_msgs::MobilityResponse& msg); - - /** - * \brief Function to process mobility response in leader waiting state - * - * \param msg incoming mobility response - */ - void mob_resp_cb_leaderwaiting(const cav_msgs::MobilityResponse& msg); - - /** - * \brief Function to process mobility response in follower state - * - * \param msg incoming mobility response - */ - void mob_resp_cb_follower(const cav_msgs::MobilityResponse& msg); - - /** - * \brief Function to process mobility response in candidate follower state - * - * \param msg incoming mobility response - */ - void mob_resp_cb_candidatefollower(const cav_msgs::MobilityResponse& msg); - - /** - * \brief Function to process mobility response in standby state - * - * \param msg incoming mobility response - */ - void mob_resp_cb_standby(const cav_msgs::MobilityResponse& msg); - - /** - * \brief Function to process mobility operation in leader state - * - * \param msg incoming mobility operation - */ - void mob_op_cb_leader(const cav_msgs::MobilityOperation& msg); - - /** - * \brief Function to process mobility operation in leader waiting state - * - * \param msg incoming mobility operation - */ - void mob_op_cb_leaderwaiting(const cav_msgs::MobilityOperation& msg); - - /** - * \brief Function to process mobility operation in follower state - * - * \param msg incoming mobility operation - */ - void mob_op_cb_follower(const cav_msgs::MobilityOperation& msg); - - /** - * \brief Function to process mobility operation in candidate follower state - * - * \param msg incoming mobility operation - */ - void mob_op_cb_candidatefollower(const cav_msgs::MobilityOperation& msg); - - /** - * \brief Function to process mobility operation in standby state - * - * \param msg incoming mobility operation - */ - void mob_op_cb_standby(const cav_msgs::MobilityOperation& msg); - - /** - * \brief Function to compose mobility operation in leader state - * - * \param type type of mobility operation (info or status) - * - * \return mobility operation msg - */ - cav_msgs::MobilityOperation composeMobilityOperationLeader(const std::string& type); - - /** - * \brief Function to compose mobility operation in follower state - * - * \return mobility operation msg - */ - cav_msgs::MobilityOperation composeMobilityOperationFollower(); - - /** - * \brief Function to compose mobility operation in leader waiting state - * - * \param type type of mobility operation (status or join_requirements) - * - * \return mobility operation msg - */ - cav_msgs::MobilityOperation composeMobilityOperationLeaderWaiting(const std::string& type); - - /** - * \brief Function to compose mobility operation in candidate follower state - * - * \return mobility operation msg - */ - cav_msgs::MobilityOperation composeMobilityOperationCandidateFollower(); - - /** - * \brief Function to convert pose from map frame to ecef location - * - * \param pose_msg pose message - * - * \return mobility operation msg - */ - cav_msgs::LocationECEF pose_to_ecef(geometry_msgs::PoseStamped pose_msg); - - /** - * \brief Function to convert ecef location to a 2d point in map frame - * - * \param ecef_point ecef location point - * - * \return 2d point in map frame - */ - lanelet::BasicPoint2d ecef_to_map_point(cav_msgs::LocationECEF ecef_point); - - - - // flag to check if map is loaded - bool map_loaded_ = false; - - // Pointer for map projector - std::shared_ptr map_projector_; - - // Flag to indicate that Leader must perform a left lane change into a suitable platooning lane before responding ACK to a 'JOIN_PLATOON_AT_REAR' request - // Note: Leader must be on a single-lane road or in a non-rightmost-lane (of a given travel direction) before forming an initial platoon - bool leader_lane_change_required_ = false; - - // Flag to indicate that CandidateFollower must change lanes into another lane (provided by the target Leader) to join target Leader's platoon - bool cf_lane_change_required_ = false; - - // ros service servers - ros::ServiceServer maneuver_srv_; - - // Plugin discovery message - cav_msgs::Plugin plugin_discovery_msg_; - - double maxAllowedJoinTimeGap_ = 15.0; - double maxAllowedJoinGap_ = 90; - int maxPlatoonSize_ = 10; - double vehicleLength_ = 5.0; - int infoMessageInterval_ = 200; //ms - long lastHeartBeatTime = 0.0; - int statusMessageInterval_ = 100; //ms - int NEGOTIATION_TIMEOUT = 5000; // ms - int noLeaderUpdatesCounter = 0; - int LEADER_TIMEOUT_COUNTER_LIMIT = 5; - double waitingStateTimeout = 25.0; // s - double desiredJoinGap = 30.0; // m - double desiredJoinTimeGap = 4.0; // s - - - // Strategy types - const std::string MOBILITY_STRATEGY = "Carma/Platooning"; - const std::string OPERATION_INFO_TYPE = "INFO"; - const std::string OPERATION_STATUS_TYPE = "STATUS"; - const std::string OPERATION_JOIN_REQUIREMENTS_TYPE = "JOIN_REQUIREMENTS"; - const std::string OPERATION_INFO_PARAMS = "INFO|REAR:%s,LENGTH:%.2f,SPEED:%.2f,SIZE:%d,DTD:%.2f,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f"; - const std::string OPERATION_STATUS_PARAMS = "STATUS|CMDSPEED:%1%,DTD:%2%,SPEED:%3%,ECEFX:%4%,ECEFY:%5%,ECEFZ:%6%"; - const std::string OPERATION_JOIN_REQUIREMENTS_PARAMS = "JOIN_REQUIREMENTS|LANE_INDEX:%1%,LANE_GROUP_SIZE:%2%"; // 'Join' requirements sent to platoon applicant when in LeaderWaiting state - const std::string JOIN_AT_REAR_PARAMS = "SIZE:%1%,SPEED:%2%,DTD:%3%,ECEFX:%4%,ECEFY:%5%,ECEFZ:%6%"; - - - std::string bsmIDtoString(cav_msgs::BSMCoreData bsm_core) - { - std::string res = ""; - for (size_t i=0; i -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "platoon_strategic.h" -#include "platoon_config.h" - -namespace platoon_strategic -{ -/** - * \brief ROS node for the platoon_strategic plugin - */ -class PlatoonStrategicPluginNode -{ -public: - - /** - * \brief Entrypoint for this node - */ - void run() - { - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - carma_wm::WMListener wml; - auto wm_ = wml.getWorldModel(); - - ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); - ros::Publisher mob_response_pub = nh.advertise("outgoing_mobility_response", 1); - ros::Publisher mob_request_pub = nh.advertise("outgoing_mobility_request", 1); - ros::Publisher mob_operation_pub = nh.advertise("outgoing_mobility_operation", 1); - ros::Publisher platoon_info_pub = nh.advertise("platoon_info", 1); - - - PlatoonPluginConfig config; - - pnh.param("maxPlatoonSize", config.maxPlatoonSize, config.maxPlatoonSize); - pnh.param("algorithmType", config.algorithmType, config.algorithmType); - pnh.param("statusMessageInterval", config.statusMessageInterval, config.statusMessageInterval); - pnh.param("infoMessageInterval", config.infoMessageInterval, config.infoMessageInterval); - pnh.param("timeHeadway", config.timeHeadway, config.timeHeadway); - pnh.param("standStillHeadway", config.standStillHeadway, config.standStillHeadway); - pnh.param("maxAllowedJoinTimeGap", config.maxAllowedJoinTimeGap, config.maxAllowedJoinTimeGap); - pnh.param("maxAllowedJoinGap", config.maxAllowedJoinGap, config.maxAllowedJoinGap); - pnh.param("desiredJoinTimeGap", config.desiredJoinTimeGap, config.desiredJoinTimeGap); - pnh.param("desiredJoinGap", config.desiredJoinGap, config.desiredJoinGap); - pnh.param("waitingStateTimeout", config.waitingStateTimeout, config.waitingStateTimeout); - pnh.param("cmdSpeedMaxAdjustment", config.cmdSpeedMaxAdjustment, config.cmdSpeedMaxAdjustment); - pnh.param("lowerBoundary", config.lowerBoundary, config.lowerBoundary); - pnh.param("upperBoundary", config.upperBoundary, config.upperBoundary); - pnh.param("maxSpacing", config.maxSpacing, config.maxSpacing); - pnh.param("minSpacing", config.minSpacing, config.minSpacing); - pnh.param("minGap", config.minGap, config.minGap); - pnh.param("maxGap", config.maxGap, config.maxGap); - pnh.param("maxCrosstrackError", config.maxCrosstrackError, config.maxCrosstrackError); - pnh.getParam("/vehicle_length", config.vehicleLength); - pnh.getParam("/vehicle_id", config.vehicleID); - - ROS_INFO_STREAM("PlatoonPluginConfig Params" << config); - - PlatoonStrategicPlugin worker(wm_, config, [&discovery_pub](auto msg) { discovery_pub.publish(msg); }, [&mob_response_pub](auto msg) { mob_response_pub.publish(msg); }, - [&mob_request_pub](auto msg) { mob_request_pub.publish(msg); }, [&mob_operation_pub](auto msg) { mob_operation_pub.publish(msg); }, - [&platoon_info_pub](auto msg) { platoon_info_pub.publish(msg); } ); - - ros::ServiceServer maneuver_srv_ = nh.advertiseService("platoon_strategic/plan_maneuvers", - &PlatoonStrategicPlugin::plan_maneuver_cb, &worker); - ros::Subscriber mob_request_sub = nh.subscribe("incoming_mobility_request", 1, &PlatoonStrategicPlugin::mob_req_cb, &worker); - ros::Subscriber mob_response_sub = nh.subscribe("incoming_mobility_response", 1, &PlatoonStrategicPlugin::mob_resp_cb, &worker); - ros::Subscriber mob_operation_sub = nh.subscribe("incoming_mobility_operation", 1, &PlatoonStrategicPlugin::mob_op_cb, &worker); - ros::Subscriber current_pose_sub = nh.subscribe("current_pose", 1, &PlatoonStrategicPlugin::pose_cb, &worker); - ros::Subscriber current_twist_sub = nh.subscribe("current_velocity", 1, &PlatoonStrategicPlugin::twist_cb, &worker); - ros::Subscriber bsm_sub = nh.subscribe("bsm_outbound", 1, &PlatoonStrategicPlugin::bsm_cb, &worker); - ros::Subscriber cmd_sub = nh.subscribe("twist_raw", 1, &PlatoonStrategicPlugin::cmd_cb, &worker); - ros::Subscriber georeference_sub = nh.subscribe("georeference", 1, &PlatoonStrategicPlugin::georeference_cb, &worker); - - - ros::Timer discovery_pub_timer_ = nh.createTimer( - ros::Duration(ros::Rate(10.0)), - [&worker](const auto&) { worker.onSpin(); }); - - ros::CARMANodeHandle::spin(); - } -}; - -} // namespace platoon_strategic \ No newline at end of file diff --git a/platooning_strategic/launch/platoon_strategic.launch b/platooning_strategic/launch/platoon_strategic.launch deleted file mode 100644 index f59901d464..0000000000 --- a/platooning_strategic/launch/platoon_strategic.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - diff --git a/platooning_strategic/package.xml b/platooning_strategic/package.xml deleted file mode 100644 index de3c4de2d8..0000000000 --- a/platooning_strategic/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - platoon_strategic - 3.3.0 - The node is to plan a platooning maneuver - carma - Apache 2.0 License - catkin - carma_utils - cav_msgs - roscpp - std_msgs - carma_wm - cav_srvs - lanelet2_extension - tf - tf2 - tf2_ros - tf2_geometry_msgs - carma_cmake_common - diff --git a/platooning_strategic/src/main.cpp b/platooning_strategic/src/main.cpp deleted file mode 100644 index 57d1750985..0000000000 --- a/platooning_strategic/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 "platoon_strategic_plugin_node.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "platoon_strategic"); - platoon_strategic::PlatoonStrategicPluginNode node; - node.run(); - return 0; -}; diff --git a/platooning_strategic/src/platoon_manager.cpp b/platooning_strategic/src/platoon_manager.cpp deleted file mode 100644 index 4f06f71447..0000000000 --- a/platooning_strategic/src/platoon_manager.cpp +++ /dev/null @@ -1,433 +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 "platoon_manager.h" -#include "platoon_config.h" -#include -#include -#include - - -namespace platoon_strategic -{ - PlatoonManager::PlatoonManager() - {} - - - void PlatoonManager::memberUpdates(const std::string& senderId, const std::string& platoonId, const std::string& senderBsmId,const std::string& params, const double& DtD){ - - std::vector inputsParams; - boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); - - std::vector cmd_parsed; - boost::algorithm::split(cmd_parsed, inputsParams[0], boost::is_any_of(":")); - double cmdSpeed = std::stod(cmd_parsed[1]); - ROS_DEBUG_STREAM("Command Speed: " << cmdSpeed); - - - std::vector dtd_parsed; - boost::algorithm::split(dtd_parsed, inputsParams[1], boost::is_any_of(":")); - double dtDistance = std::stod(dtd_parsed[1]); - ROS_DEBUG_STREAM("Downtrack Distance: " << dtDistance); - // get DtD directly instead of parsing message - dtDistance = DtD; - ROS_DEBUG_STREAM("Downtrack Distance ecef: " << dtDistance); - - std::vector cur_parsed; - boost::algorithm::split(cur_parsed, inputsParams[2], boost::is_any_of(":")); - double curSpeed = std::stod(cur_parsed[1]); - ROS_DEBUG_STREAM("Current Speed Speed: " << curSpeed); - - - // If we are currently in a follower state: - // 1. We will update platoon ID based on leader's STATUS - // 2. We will update platoon members info based on platoon ID if it is in front of us - if(isFollower) - { - - bool isFromLeader = (leaderID == senderId); - - bool needPlatoonIdChange = isFromLeader && (currentPlatoonID != platoonId); - - bool isVehicleInFrontOf = (dtDistance >= getCurrentDowntrackDistance()); - - if(needPlatoonIdChange) { - ROS_DEBUG_STREAM("It seems that the current leader is joining another platoon."); - ROS_DEBUG_STREAM("So the platoon ID is changed from " << currentPlatoonID << " to " << platoonId); - currentPlatoonID = platoonId; - updatesOrAddMemberInfo(senderId, senderBsmId, cmdSpeed, dtDistance, curSpeed); - } - else if((currentPlatoonID == platoonId) && isVehicleInFrontOf) - { - ROS_DEBUG_STREAM("This STATUS messages is from our platoon in front of us. Updating the info..."); - updatesOrAddMemberInfo(senderId, senderBsmId, cmdSpeed, dtDistance, curSpeed); - leaderID = (platoon.size()==0) ? HostMobilityId : platoon[0].staticId; - ROS_DEBUG_STREAM("The first vehicle in our list is now " << leaderID); - } - else - { - ROS_DEBUG_STREAM("This STATUS message is not from our platoon. We ignore this message with id: " << senderId); - } - } - else - { - // If we are currently in any leader state, we only updates platoon member based on platoon ID - if (currentPlatoonID == platoonId) - { - ROS_DEBUG_STREAM("This STATUS messages is from our platoon. Updating the info..."); - updatesOrAddMemberInfo(senderId, senderBsmId, cmdSpeed, dtDistance, curSpeed); - } - } - - } - - - - void PlatoonManager::updatesOrAddMemberInfo(std::string senderId, std::string senderBsmId, double cmdSpeed, double dtDistance, double curSpeed) { - - bool isExisted = false; - // update/add this info into the list - for (PlatoonMember& pm : platoon){ - if(pm.staticId == senderId) { - pm.bsmId = senderBsmId; - pm.commandSpeed = cmdSpeed; - pm.vehiclePosition = dtDistance; - pm.vehicleSpeed = curSpeed; - pm.timestamp = ros::Time::now().toNSec()/1000000; - ROS_DEBUG_STREAM("Receive and update platooning info on vehicel " << pm.staticId); - ROS_DEBUG_STREAM(" BSM ID = " << pm.bsmId); - ROS_DEBUG_STREAM(" Speed = " << pm.vehicleSpeed); - ROS_DEBUG_STREAM(" Location = " << pm.vehiclePosition); - ROS_DEBUG_STREAM(" CommandSpeed = " << pm.commandSpeed); - isExisted = true; - break; - } - } - - if(!isExisted) { - long cur_t = ros::Time::now().toNSec()/1000000; // time in millisecond - - PlatoonMember newMember = PlatoonMember(senderId, senderBsmId, cmdSpeed, curSpeed, dtDistance, cur_t); - platoon.push_back(newMember); - - std::sort(std::begin(platoon), std::end(platoon), [](const PlatoonMember &a, const PlatoonMember &b){return a.vehiclePosition < b.vehiclePosition;}); - - ROS_DEBUG_STREAM("Add a new vehicle into our platoon list " << newMember.staticId); - } - } - - - - int PlatoonManager::getTotalPlatooningSize() const{ - if(isFollower) { - return platoonSize; - } - return platoon.size() + 1; - } - - double PlatoonManager::getPlatoonRearDowntrackDistance(){ - if(platoon.size() < 1) - { - - double dist = getCurrentDowntrackDistance(); - return dist; - } - return platoon[platoon.size() - 1].vehiclePosition; - } - - PlatoonMember PlatoonManager::getLeader(){ - PlatoonMember leader; - ROS_DEBUG_STREAM("platoon size: " << platoon.size()); - if(isFollower && platoon.size() != 0) - { - ROS_DEBUG_STREAM("Leader initially set as first vehicle in platoon"); - // return the first vehicle in the platoon as default if no valid algorithm applied - leader = platoon[0]; - if (algorithmType_ == "APF_ALGORITHM"){ - int newLeaderIndex = allPredecessorFollowing(); - if(newLeaderIndex < platoon.size() && newLeaderIndex >= 0) { - leader = platoon[newLeaderIndex]; - ROS_DEBUG_STREAM("APF output: " << leader.staticId); - previousFunctionalLeaderIndex_ = newLeaderIndex; - previousFunctionalLeaderID_ = leader.staticId; - } - else { - // it might happened when the subject vehicle gets far away from the preceding vehicle so we follow the one in front - leader = platoon[platoon.size() - 1]; - previousFunctionalLeaderIndex_ = platoon.size() - 1; - previousFunctionalLeaderID_ = leader.staticId; - ROS_DEBUG_STREAM("Based on the output of APF algorithm we start to follow our predecessor."); - } - } - } - return leader; - - } - - int PlatoonManager::allPredecessorFollowing(){ - ///***** Case Zero *****/// - // If we are the second vehicle in this platoon,we will always follow the leader vehicle - if(platoon.size() == 1) { - ROS_DEBUG("As the second vehicle in the platoon, it will always follow the leader. Case Zero"); - return 0; - } - ///***** Case One *****/// - // If we do not have a leader in the previous time step, we follow the first vehicle as default - if(previousFunctionalLeaderID_ == "") { - ROS_DEBUG("APF algorithm did not found a leader in previous time step. Case one"); - return 0; - } - // Generate an array of downtrack distance for every vehicles in this platoon including the host vehicle - // The size of distance array is platoon.size() + 1, because the platoon list did not contain the host vehicle - std::vector downtrackDistance(platoon.size() + 1); - for(int i = 0; i < platoon.size(); i++) { - downtrackDistance[i] = platoon[i].vehiclePosition; - } - - double dt = getCurrentDowntrackDistance(); - downtrackDistance[downtrackDistance.size() - 1] = dt; - - // Generate an array of speed for every vehicles in this platoon including the host vehicle - // The size of speed array is platoon.size() + 1, because the platoon list did not contain the host vehicle - std::vector speed(platoon.size() + 1); - for(int i = 0; i < platoon.size(); i++) { - speed[i] = platoon[i].vehicleSpeed; - } - - double cur_speed = getCurrentSpeed(); - speed[speed.size() - 1] = cur_speed; - - ///***** Case Two *****/// - // If the distance headway between the subject vehicle and its predecessor is an issue - // according to the "min_gap" and "max_gap" thresholds, then it should follow its predecessor - // The following line will not throw exception because the length of downtrack array is larger than two in this case - double timeHeadwayWithPredecessor = downtrackDistance[downtrackDistance.size() - 2] - downtrackDistance[downtrackDistance.size() - 1]; - gapWithFront_ = timeHeadwayWithPredecessor; - if(insufficientGapWithPredecessor(timeHeadwayWithPredecessor)) { - ROS_DEBUG("APF algorithm decides there is an issue with the gap with preceding vehicle: " , timeHeadwayWithPredecessor , ". Case Two"); - return platoon.size() - 1; - } - else{ - // implementation of the main part of APF algorithm - // calculate the time headway between every consecutive pair of vehicles - std::vector timeHeadways = calculateTimeHeadway(downtrackDistance, speed); - ROS_DEBUG("APF calculate time headways: " ); - for (const auto& value : timeHeadways) - { - ROS_DEBUG("APF time headways: " , value); - } - ROS_DEBUG("APF found the previous leader is " , previousFunctionalLeaderID_); - // if the previous leader is the first vehicle in the platoon - - if(previousFunctionalLeaderIndex_ == 0) { - ///***** Case Three *****/// - // If there is a violation, the return value is the desired leader index - ROS_DEBUG("APF use violations on lower boundary or maximum spacing to choose leader. Case Three."); - return determineLeaderBasedOnViolation(timeHeadways); - } - else{ - // if the previous leader is not the first vehicle - // get the time headway between every consecutive pair of vehicles from indexOfPreviousLeader - std::vector partialTimeHeadways = getTimeHeadwayFromIndex(timeHeadways, previousFunctionalLeaderIndex_); - ROS_DEBUG("APF partial time headways array:: " ); - for (const auto& value : partialTimeHeadways) - { - ROS_DEBUG("APF partial time headways: " , value); - } - int closestLowerBoundaryViolation = findLowerBoundaryViolationClosestToTheHostVehicle(partialTimeHeadways); - int closestMaximumSpacingViolation = findMaximumSpacingViolationClosestToTheHostVehicle(partialTimeHeadways); - // if there are no violations anywhere between the subject vehicle and the current leader, - // then depending on the time headways of the ENTIRE platoon, the subject vehicle may switch - // leader further downstream. This is because the subject vehicle has determined that there are - // no time headways between itself and the current leader which would cause the platoon to be unsafe. - // if there are violations somewhere betweent the subject vehicle and the current leader, - // then rather than assigning leadership further DOWNSTREAM, we must go further UPSTREAM in the following lines - if(closestLowerBoundaryViolation == -1 && closestMaximumSpacingViolation == -1) { - // In order for the subject vehicle to assign leadership further downstream, - // two criteria must be satisfied: first the leading vehicle and its immediate follower must - // have a time headway greater than "upper_boundary." The purpose of this criteria is to - // introduce a hysteresis in order to eliminate the possibility of a vehicle continually switching back - // and forth between two leaders because one of the time headways is hovering right around - // the "lower_boundary" threshold; second the leading vehicle and its predecessor must have - // a time headway less than "min_spacing" second. Just as with "upper_boundary", "min_spacing" exists to - // introduce a hysteresis where leaders are continually being switched. - bool condition1 = timeHeadways[previousFunctionalLeaderIndex_] > upperBoundary_; - bool condition2 = timeHeadways[previousFunctionalLeaderIndex_ - 1] < minSpacing_; - ///***** Case Four *****/// - //we may switch leader further downstream - if(condition1 && condition2) { - ROS_DEBUG("APF found two conditions for assigning leadership further downstream are satisfied. Case Four"); - return determineLeaderBasedOnViolation(timeHeadways); - } else { - ///***** Case Five *****/// - // We may not switch leadership to another vehicle further downstream because some criteria are not satisfied - ROS_DEBUG("APF found two conditions for assigning leadership further downstream are not satisfied. Case Five."); - ROS_DEBUG("condition1: " , condition1 , " & condition2: " , condition2); - return previousFunctionalLeaderIndex_; - } - } else if(closestLowerBoundaryViolation != -1 && closestMaximumSpacingViolation == -1) { - // The rest four cases have roughly the same logic: locate the closest violation and assign leadership accordingly - ///***** Case Six *****/// - ROS_DEBUG("APF found closestLowerBoundaryViolation on partial time headways. Case Six."); - return previousFunctionalLeaderIndex_ - 1 + closestLowerBoundaryViolation; - - } else if(closestLowerBoundaryViolation == -1 && closestMaximumSpacingViolation != -1) { - ///***** Case Seven *****/// - ROS_DEBUG("APF found closestMaximumSpacingViolation on partial time headways. Case Seven."); - return previousFunctionalLeaderIndex_ + closestMaximumSpacingViolation; - } else{ - ROS_DEBUG("APF found closestMaximumSpacingViolation and closestLowerBoundaryViolation on partial time headways."); - if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) { - ///***** Case Eight *****/// - ROS_DEBUG("closest LowerBoundaryViolation is higher than closestMaximumSpacingViolation on partial time headways. Case Eight."); - return previousFunctionalLeaderIndex_ - 1 + closestLowerBoundaryViolation; - } else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation) { - ///***** Case Nine *****/// - ROS_DEBUG("closestMaximumSpacingViolation is higher than closestLowerBoundaryViolation on partial time headways. Case Nine."); - return previousFunctionalLeaderIndex_ + closestMaximumSpacingViolation; - } else { - ROS_DEBUG("APF Leader selection parameter is wrong!"); - return 0; - } - } - } - - } - } - - std::vector PlatoonManager::getTimeHeadwayFromIndex(std::vector timeHeadways, int start) const { - std::vector result(timeHeadways.begin() + start-1, timeHeadways.end()); - return result; - } - - - bool PlatoonManager::insufficientGapWithPredecessor(double distanceToFrontVehicle) { - bool frontGapIsTooSmall = distanceToFrontVehicle < minGap_; - bool previousLeaderIsPredecessor = (previousFunctionalLeaderID_ == platoon[platoon.size() - 1].staticId); - bool frontGapIsNotLargeEnough = (distanceToFrontVehicle < maxGap_ && previousLeaderIsPredecessor); - return (frontGapIsTooSmall || frontGapIsNotLargeEnough); - } - - std::vector PlatoonManager::calculateTimeHeadway(std::vector downtrackDistance, std::vector speed) const{ - std::vector timeHeadways(downtrackDistance.size() - 1); - for (int i=0; i::infinity(); - } - } - return timeHeadways; - } - - - int PlatoonManager::determineLeaderBasedOnViolation(std::vector timeHeadways){ - int closestLowerBoundaryViolation = findLowerBoundaryViolationClosestToTheHostVehicle(timeHeadways); - int closestMaximumSpacingViolation = findMaximumSpacingViolationClosestToTheHostVehicle(timeHeadways); - if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) { - ROS_DEBUG("APF found violation on closestLowerBoundaryViolation at " , closestLowerBoundaryViolation); - return closestLowerBoundaryViolation; - } else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation){ - ROS_DEBUG("APF found violation on closestMaximumSpacingViolation at " , closestMaximumSpacingViolation); - return closestMaximumSpacingViolation + 1; - } - else{ - ROS_DEBUG("APF found no violations on both closestLowerBoundaryViolation and closestMaximumSpacingViolation"); - return 0; - } - } - - // helper method for APF algorithm - int PlatoonManager::findLowerBoundaryViolationClosestToTheHostVehicle(std::vector timeHeadways) const{ - for(int i = timeHeadways.size() - 1; i >= 0; i--) { - if(timeHeadways[i] < lowerBoundary_) { - return i; - } - } - return -1; - } - - // helper method for APF algorithm - int PlatoonManager::findMaximumSpacingViolationClosestToTheHostVehicle(std::vector timeHeadways) const { - for(int i = timeHeadways.size() - 1; i >= 0; i--) { - if(timeHeadways[i] > maxSpacing_) { - return i; - } - } - return -1; - } - - void PlatoonManager::changeFromFollowerToLeader() { - isFollower = false; - platoon = {}; - leaderID = HostMobilityId; - currentPlatoonID = boost::uuids::to_string(boost::uuids::random_generator()()); - previousFunctionalLeaderID_ = ""; - previousFunctionalLeaderIndex_ = -1; - ROS_DEBUG("The platoon manager is changed from follower state to leader state."); - } - - void PlatoonManager::changeFromLeaderToFollower(std::string newPlatoonId) { - isFollower = true; - currentPlatoonID = newPlatoonId; - platoon = {}; - ROS_DEBUG("The platoon manager is changed from leader state to follower state."); - } - - int PlatoonManager::getNumberOfVehicleInFront() { - if(isFollower) { - return platoon.size(); - } - return 0; - } - - - //Currently not used in implementation - double PlatoonManager::getDistanceFromRouteStart() const{ - return current_downtrack_distance_; - } - - double PlatoonManager::getDistanceToFrontVehicle() { - return gapWithFront_; - } - - double PlatoonManager::getCurrentSpeed() const { - return current_speed_; - } - - double PlatoonManager::getCommandSpeed(){ - return command_speed_; - } - - double PlatoonManager::getCurrentDowntrackDistance() const - { - double current_progress = current_downtrack_distance_; - return current_progress; - } - - - - double PlatoonManager::getCurrentPlatoonLength() { - if(platoon.size() == 0) { - return vehicleLength_; - } else { - return getCurrentDowntrackDistance() - platoon[platoon.size() - 1].vehiclePosition + vehicleLength_; - } - } - - -} diff --git a/platooning_strategic/src/platoon_strategic.cpp b/platooning_strategic/src/platoon_strategic.cpp deleted file mode 100644 index 49b61ee736..0000000000 --- a/platooning_strategic/src/platoon_strategic.cpp +++ /dev/null @@ -1,1603 +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 -#include "platoon_strategic.h" - - -namespace platoon_strategic -{ - PlatoonStrategicPlugin::PlatoonStrategicPlugin() - { - } - - PlatoonStrategicPlugin::PlatoonStrategicPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher, MobilityResponseCB mobility_response_publisher, - MobilityRequestCB mobility_request_publisher, MobilityOperationCB mobility_operation_publisher, - PlatooningInfoCB platooning_info_publisher) - : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher), - mobility_response_publisher_(mobility_response_publisher), mobility_request_publisher_(mobility_request_publisher), - mobility_operation_publisher_(mobility_operation_publisher), platooning_info_publisher_(platooning_info_publisher) - { - pm_ = PlatoonManager(); - - plugin_discovery_msg_.name = "platoon_strategic"; - plugin_discovery_msg_.version_id = "v1.0"; - plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = true; - plugin_discovery_msg_.type = cav_msgs::Plugin::STRATEGIC; - plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers"; - } - - - - bool PlatoonStrategicPlugin::onSpin() - { - plugin_discovery_publisher_(plugin_discovery_msg_); - - if (pm_.current_platoon_state == PlatoonState::LEADER) - { - run_leader(); - } - else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) - { - run_follower(); - } - else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) - { - run_candidate_follower(); - } - else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) - { - run_leader_waiting(); - } - - cav_msgs::PlatooningInfo platoon_status = composePlatoonInfoMsg(); - platooning_info_publisher_(platoon_status); - - if (!platooning_enabled_) - { - pm_.current_platoon_state = PlatoonState::STANDBY; - return true; - } - - return true; - } - - - - - - int PlatoonStrategicPlugin::findLaneletIndexFromPath(int target_id, lanelet::routing::LaneletPath& path) - { - for(size_t i = 0; i < path.size(); ++i) - { - if(path[i].id() == target_id) - { - return i; - } - } - return -1; - } - - void PlatoonStrategicPlugin::bsm_cb(const cav_msgs::BSMConstPtr& msg) - { - cav_msgs::BSMCoreData bsm_core_ = msg->core_data; - host_bsm_id_ = bsmIDtoString(bsm_core_); - } - - void PlatoonStrategicPlugin::pose_cb(const geometry_msgs::PoseStampedConstPtr& msg) - { - pose_msg_ = geometry_msgs::PoseStamped(*msg.get()); - - if (pm_.current_platoon_state != PlatoonState::STANDBY) - { - lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y); - - carma_wm::TrackPos tc = wm_->routeTrackPos(current_loc); - current_downtrack_ = tc.downtrack; - ROS_DEBUG_STREAM("current_downtrack_ = " << current_downtrack_); - current_crosstrack_ = tc.crosstrack; - ROS_DEBUG_STREAM("current_crosstrack_ = " << current_crosstrack_); - - pose_ecef_point_ = pose_to_ecef(pose_msg_); - - checkForRightMostLane(current_loc); - - current_lane_index_ = getCurrentLaneIndex(current_loc); - - current_lane_group_size_ = getCurrentLaneGroupSize(current_loc); - } - - } - - void PlatoonStrategicPlugin::cmd_cb(const geometry_msgs::TwistStampedConstPtr& msg) - { - cmd_speed_ = msg->twist.linear.x; - } - - void PlatoonStrategicPlugin::twist_cb(const geometry_msgs::TwistStampedConstPtr& msg) - { - current_speed_ = msg->twist.linear.x; - if (current_speed_ < 0.01) - { - current_speed_ = 0.0; - } - } - - void PlatoonStrategicPlugin::checkForRightMostLane(const lanelet::BasicPoint2d& current_location) - { - auto current_lanelet = wm_->getLaneletsFromPoint(current_location, 1); - if (current_lanelet.size()<1) - { - throw std::invalid_argument("There are no lanelets in the current location."); - } - ROS_DEBUG_STREAM("current_lanelet" << current_lanelet[0].id()); - auto routing_graph = wm_->getMapRoutingGraph(); - auto right_lanelet = routing_graph->right(current_lanelet[0]); - if (!right_lanelet) - { - in_rightmost_lane_ = true; - ROS_DEBUG_STREAM("Vehicle is in the rightmost lane"); - } - else - { - in_rightmost_lane_ = false; - ROS_DEBUG_STREAM("Vehicle is NOT in the rightmost lane"); - } - - auto left_lanelet = routing_graph->left(current_lanelet[0]); - if (!left_lanelet && in_rightmost_lane_) - { - single_lane_road_ = true; - ROS_DEBUG_STREAM("Vehicle is in a single-lane road"); - } - else - { - single_lane_road_ = false; - ROS_DEBUG_STREAM("Vehicle is NOT in a single-lane road"); - } - } - - int PlatoonStrategicPlugin::getCurrentLaneIndex(const lanelet::BasicPoint2d& current_location) - { - auto current_lanelet = wm_->getLaneletsFromPoint(current_location, 1); - if (current_lanelet.size()<1) - { - throw std::invalid_argument("There are no lanelets in the current location."); - } - ROS_DEBUG_STREAM("current_lanelet" << current_lanelet[0].id()); - auto routing_graph = wm_->getMapRoutingGraph(); - - // Obtain the current local lane index (0 is rightmost lane, 1 is second rightmost, etc.); considers only the current travel direction - int lane_index = (routing_graph->rights(current_lanelet[0])).size(); - - return lane_index; - } - - int PlatoonStrategicPlugin::getCurrentLaneGroupSize(const lanelet::BasicPoint2d& current_location) - { - auto current_lanelet = wm_->getLaneletsFromPoint(current_location, 1); - if (current_lanelet.size()<1) - { - throw std::invalid_argument("There are no lanelets in the current location."); - } - ROS_DEBUG_STREAM("current_lanelet" << current_lanelet[0].id()); - auto routing_graph = wm_->getMapRoutingGraph(); - - // Obtain the current local lane index (0 is rightmost lane, 1 is second rightmost, etc.); considers only the current travel direction - int lane_quantity_right = (routing_graph->rights(current_lanelet[0])).size(); - int lane_quantity_left = (routing_graph->lefts(current_lanelet[0])).size(); - int lane_quantity = lane_quantity_right + lane_quantity_left + 1; // Include the '+1' to account for the current location's lane - - return lane_quantity; - } - - void PlatoonStrategicPlugin::updateCurrentStatus(cav_msgs::Maneuver maneuver, double& speed, double& current_progress, int& lane_id){ - if(maneuver.type == cav_msgs::Maneuver::LANE_FOLLOWING){ - speed = maneuver.lane_following_maneuver.end_speed; - current_progress = maneuver.lane_following_maneuver.end_dist; - if (maneuver.lane_following_maneuver.lane_ids.empty()) { - ROS_WARN_STREAM("Lane id of lane following maneuver not set. Using 0"); - lane_id = 0; - } else { - lane_id = stoi(maneuver.lane_following_maneuver.lane_ids[0]); - } - } - else - { - ROS_WARN_STREAM("Detected a maneuver other than LANE_FOLLOWING, which is currently not supported. Using 0 index..."); - lane_id = 0; - } - } - - - - bool PlatoonStrategicPlugin::plan_maneuver_cb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp) - { - - lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y); - auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 10); - if(current_lanelets.size() == 0) - { - ROS_WARN_STREAM("Cannot find any lanelet in map!"); - return true; - } - - auto shortest_path = wm_->getRoute()->shortestPath(); - - lanelet::ConstLanelet current_lanelet; - int last_lanelet_index = -1; - for (auto llt : current_lanelets) - { - if (boost::geometry::within(current_loc, llt.second.polygon2d())) - { - int potential_index = findLaneletIndexFromPath(llt.second.id(), shortest_path); - if (potential_index != -1) - { - last_lanelet_index = potential_index; - current_lanelet = shortest_path[last_lanelet_index]; - break; - } - } - } - if(last_lanelet_index == -1) - { - ROS_ERROR_STREAM("Current position is not on the shortest path! Returning an empty maneuver"); - return true; - } - double current_progress = wm_->routeTrackPos(current_loc).downtrack; - double speed_progress = current_speed_; - ros::Time time_progress = ros::Time::now(); - double target_speed = findSpeedLimit(current_lanelet); //get Speed Limit TOTO update - double total_maneuver_length = current_progress + config_.mvr_duration * target_speed; - double route_length = wm_->getRouteEndTrackPos().downtrack; - total_maneuver_length = std::min(total_maneuver_length, route_length); - //Update current status based on prior plan - if(req.prior_plan.maneuvers.size()!=0){ - time_progress = req.prior_plan.planning_completion_time; - int end_lanelet =0; - updateCurrentStatus(req.prior_plan.maneuvers.back(),speed_progress,current_progress,end_lanelet); - if (end_lanelet == 0) - { - ROS_WARN_STREAM("Was not able to extract valid info from prior maneuver, returning..."); - return true; - } - last_lanelet_index = findLaneletIndexFromPath(end_lanelet,shortest_path); - } - - bool approaching_route_end = false; - double time_req_to_stop,stopping_dist; - - ROS_DEBUG_STREAM("Starting Loop"); - ROS_DEBUG_STREAM("total_maneuver_length: " << total_maneuver_length << " route_length: " << route_length); - auto routing_graph = wm_->getMapRoutingGraph(); - - while(current_progress < total_maneuver_length) - { - ROS_DEBUG_STREAM("Lanlet: " << shortest_path[last_lanelet_index].id()); - ROS_DEBUG_STREAM("current_progress: "<< current_progress); - ROS_DEBUG_STREAM("speed_progress: " << speed_progress); - ROS_DEBUG_STREAM("target_speed: " << target_speed); - ROS_DEBUG_STREAM("time_progress: " << time_progress.toSec()); - auto p = shortest_path[last_lanelet_index].centerline2d().back(); - double end_dist = wm_->routeTrackPos(shortest_path[last_lanelet_index].centerline2d().back()).downtrack; - end_dist = std::min(end_dist, total_maneuver_length); - ROS_DEBUG_STREAM("end_dist: " << end_dist); - double dist_diff = end_dist - current_progress; - ROS_DEBUG_STREAM("dist_diff: " << dist_diff); - if(end_dist < current_progress){ - break; - } - - if (cf_lane_change_required_ || leader_lane_change_required_) - { - ROS_DEBUG_STREAM("Planning a required lane change"); - - - auto adjacentleft_lanelet = routing_graph->left(current_lanelet); - lanelet::ConstLanelet target_lanelet; - - if (adjacentleft_lanelet) - { - target_lanelet = adjacentleft_lanelet.get(); - ROS_DEBUG_STREAM("target lanelet id for lane change: " << target_lanelet.id()); - auto lc_maneuver = composeLaneChangeManeuverMessage(current_progress, end_dist, speed_progress, target_speed, current_lanelet.id(), target_lanelet.id()); - resp.new_plan.maneuvers.push_back(lc_maneuver); - } - else - { - // In case a left lane is not detected, a lane following is maneuver is generated instead of the lane change to prevent discontinuity - // The single lane flag is also enabled to help continue platooning process. - single_lane_road_ = true; - ROS_WARN_STREAM("No adjacent left lanes exist, so no lanechange is planned"); - resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_progress, end_dist, - speed_progress, target_speed,shortest_path[last_lanelet_index].id(), time_progress)); - } - - } - else - { - resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_progress, end_dist, - speed_progress, target_speed,shortest_path[last_lanelet_index].id(), time_progress)); - } - - current_progress += dist_diff; - time_progress = GET_MANEUVER_PROPERTY(resp.new_plan.maneuvers.back(), end_time); - speed_progress = target_speed; - if(current_progress >= total_maneuver_length || last_lanelet_index == shortest_path.size() - 1) - { - break; - } - ++last_lanelet_index; - } - - if(resp.new_plan.maneuvers.size() == 0) - { - ROS_WARN_STREAM("Cannot plan maneuver because no route is found"); - } - - if (pm_.getTotalPlatooningSize() < 2) - { - resp.new_plan.maneuvers = {}; - ROS_WARN_STREAM("Platoon size 1 so Empty maneuver sent"); - } - - if (pm_.current_platoon_state == PlatoonState::STANDBY && platooning_enabled_) - { - pm_.current_platoon_state = PlatoonState::LEADER; - pm_.currentPlatoonID = boost::uuids::to_string(boost::uuids::random_generator()()); - ROS_DEBUG_STREAM("change the state from standby to leader at start-up"); - } - - pm_.current_downtrack_distance_ = current_downtrack_; - pm_.HostMobilityId = config_.vehicleID; - ROS_DEBUG_STREAM("current_downtrack: " << current_downtrack_); - return true; - } - - cav_msgs::Maneuver PlatoonStrategicPlugin::composeLaneChangeManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id) const - { - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::LANE_CHANGE; - maneuver_msg.lane_change_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::NO_NEGOTIATION; - maneuver_msg.lane_change_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_change_maneuver.parameters.planning_tactical_plugin = "cooperative_lanechange"; - maneuver_msg.lane_change_maneuver.parameters.planning_strategic_plugin = "platoon_strategic"; - maneuver_msg.lane_change_maneuver.start_dist = start_dist; - maneuver_msg.lane_change_maneuver.start_speed = start_speed; - maneuver_msg.lane_change_maneuver.end_dist = end_dist; - maneuver_msg.lane_change_maneuver.end_speed = target_speed; - maneuver_msg.lane_change_maneuver.starting_lane_id = std::to_string(starting_lane_id); - maneuver_msg.lane_change_maneuver.ending_lane_id = std::to_string(ending_lane_id); - //Start time and end time for maneuver are assigned in updateTimeProgress - - - ROS_DEBUG_STREAM("Creating lane change : " << "start dist: " << start_dist << " end dist: " << end_dist << " Starting llt: " << starting_lane_id << " Ending llt: " << ending_lane_id); - - return maneuver_msg; - } - - double PlatoonStrategicPlugin::findSpeedLimit(const lanelet::ConstLanelet& llt) - { - double target_speed = 0.0; - - lanelet::Optional traffic_rules = wm_->getTrafficRules(); - if (traffic_rules) - { - target_speed =(*traffic_rules)->speedLimit(llt).speedLimit.value(); - } - else - { - throw std::invalid_argument("Valid traffic rules object could not be built"); - } - - ROS_DEBUG_STREAM("target speed (limit) " << target_speed); - - return target_speed; - } - - cav_msgs::Maneuver PlatoonStrategicPlugin::composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time& current_time) - { - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING; - maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::PLATOONING; - maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; - maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "platooning_tactical_plugin"; - maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "platoon_strategic"; - maneuver_msg.lane_following_maneuver.start_dist = current_dist; - maneuver_msg.lane_following_maneuver.start_speed = current_speed; - maneuver_msg.lane_following_maneuver.start_time = current_time; - maneuver_msg.lane_following_maneuver.end_dist = end_dist; - maneuver_msg.lane_following_maneuver.end_speed = target_speed; - - // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet - double cur_plus_target = current_speed + target_speed; - if (cur_plus_target < 0.00001) { - maneuver_msg.lane_following_maneuver.end_time = current_time + ros::Duration(config_.mvr_duration); - } else { - maneuver_msg.lane_following_maneuver.end_time = current_time + ros::Duration((end_dist - current_dist) / (0.5 * cur_plus_target)); - } - maneuver_msg.lane_following_maneuver.lane_ids = { std::to_string(lane_id) }; - current_time = maneuver_msg.lane_following_maneuver.end_time; - ROS_DEBUG_STREAM("Creating lane follow start dist:"< waitingStateTimeout * 1000) - { - //TODO if the current state timeouts, we need to have a kind of ABORT message to inform the applicant - ROS_DEBUG_STREAM("LeaderWaitingState is timeout, changing back to PlatoonLeaderState."); - pm_.current_platoon_state = PlatoonState::LEADER; - } - // Task 2 - cav_msgs::MobilityOperation status; - status = composeMobilityOperationLeaderWaiting(OPERATION_STATUS_TYPE); - mobility_operation_publisher_(status); - ROS_DEBUG_STREAM("publish status message"); - - // Task 3: Publish JOIN_REQUIREMENTS MobilityOperation message to applicant - cav_msgs::MobilityOperation join_requirements; - join_requirements = composeMobilityOperationLeaderWaiting(OPERATION_JOIN_REQUIREMENTS_TYPE); - - mobility_operation_publisher_(join_requirements); - ROS_DEBUG_STREAM("Composed a JOIN_REQUIREMENTS MobilityOperation message with params " << join_requirements.strategy_params); - - long tsEnd = ros::Time::now().toNSec()/1000000; - long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration/1000).sleep(); - } - - void PlatoonStrategicPlugin::run_leader(){ - - long tsStart = ros::Time::now().toNSec()/1000000; - // Task 1 - bool isTimeForHeartBeat = tsStart - lastHeartBeatTime >= infoMessageInterval_; - ROS_DEBUG_STREAM("time since last heart beat: " << tsStart - lastHeartBeatTime); - if(isTimeForHeartBeat) { - cav_msgs::MobilityOperation infoOperation; - infoOperation = composeMobilityOperationLeader(OPERATION_INFO_TYPE); - mobility_operation_publisher_(infoOperation); - lastHeartBeatTime = ros::Time::now().toNSec()/1000000; - ROS_DEBUG_STREAM("Published heart beat platoon INFO mobility operatrion message"); - } - // Task 2 - // if (isTimeForHeartBeat) { - // updateLightBar(); - // } - // Task 3 - if(pm_.current_plan.valid) - { - bool isCurrentPlanTimeout = ((ros::Time::now().toNSec()/1000000 - pm_.current_plan.planStartTime) > NEGOTIATION_TIMEOUT); - if(isCurrentPlanTimeout) - { - ROS_DEBUG_STREAM("Give up current on waiting plan with planId: " << pm_.current_plan.planId); - pm_.current_plan.valid = false; - } - } - - // Task 4 - bool hasFollower = (pm_.getTotalPlatooningSize() > 1); - if(hasFollower) { - cav_msgs::MobilityOperation statusOperation; - statusOperation = composeMobilityOperationLeader(OPERATION_STATUS_TYPE); - mobility_operation_publisher_(statusOperation); - ROS_DEBUG_STREAM("Published platoon STATUS operation message"); - } - else { - // Check if Leader must change lanes into a suitable platooning lane prior before responding ACK to a JOIN_PLATOON_AT_REAR request - if (leader_lane_change_required_) { - - // If the lane change was required, check if Leader is now in a suitable platooning lane - if (!in_rightmost_lane_ || single_lane_road_) { - - // Leader is either on a single-lane road or not in the rightmost lane of a multi-lane road; it can now respond ACK to applicant's JOIN_PLATOON_AT_REAR request - cav_msgs::MobilityResponse response; - response.m_header.sender_id = config_.vehicleID; - response.m_header.recipient_id = lw_applicantId_; - response.m_header.plan_id = pm_.currentPlatoonID; - response.m_header.sender_bsm_id = host_bsm_id_; - response.m_header.timestamp = ros::Time::now().toNSec()/1000000; - response.is_accepted = true; - - ROS_DEBUG_STREAM("We are now in a suitable platooning lane, sending ACK to applicant " << lw_applicantId_); - ROS_DEBUG_STREAM("Change to LeaderWaiting State and waiting for " << lw_applicantId_ << " to join"); - pm_.current_platoon_state = PlatoonState::LEADERWAITING; - waitingStartTime = ros::Time::now().toNSec()/1000000; - mobility_response_publisher_(response); - - // Vehicle is now in a suitable platooning lane; a lane change is no longer required - leader_lane_change_required_ = false; - } - } - } - - long tsEnd = ros::Time::now().toNSec()/1000000; - long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration/1000).sleep(); - - } - - void PlatoonStrategicPlugin::run_follower(){ - // This is a interrupted-safe loop. - // This loop has four tasks: - // 1. Check the state start time, if it exceeds a limit it will give up current plan and change back to leader state - // 2. Abort current request if we wait for long enough time for response from leader and change back to leader state - // 3. Check the current distance with the target platoon rear and send out CANDIDATE-JOIN request when we get close - // 4. Publish operation status every 100 milliseconds if we still have followers - long tsStart = ros::Time::now().toNSec()/1000000; - // Job 1 - cav_msgs::MobilityOperation status; - status = composeMobilityOperationFollower(); - mobility_operation_publisher_(status); - // Job 2 - // Get the number of vehicles in this platoon who is in front of us - int vehicleInFront = pm_.getNumberOfVehicleInFront(); - if(vehicleInFront == 0) { - noLeaderUpdatesCounter++; - if(noLeaderUpdatesCounter >= LEADER_TIMEOUT_COUNTER_LIMIT) { - ROS_DEBUG_STREAM("noLeaderUpdatesCounter = " << noLeaderUpdatesCounter << " and change to leader state"); - pm_.changeFromFollowerToLeader(); - pm_.current_platoon_state = PlatoonState::LEADER; - } - } else { - // reset counter to zero when we get updates again - noLeaderUpdatesCounter = 0; - } - long tsEnd = ros::Time::now().toNSec()/1000000; - long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration/1000).sleep(); - } - - void PlatoonStrategicPlugin::run_candidate_follower(){ - long tsStart = ros::Time::now().toNSec()/1000000; - // Task 1 - bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000; - ROS_DEBUG_STREAM("timeout1: " << tsStart - candidatestateStartTime); - ROS_DEBUG_STREAM("waitingStateTimeout: " << waitingStateTimeout*1000); - if(isCurrentStateTimeout) { - ROS_DEBUG_STREAM("The current candidate follower state is timeout. Change back to leader state."); - pm_.current_platoon_state = PlatoonState::LEADER; - } - // Task 2 - - if(pm_.current_plan.valid) { - { - if(pm_.current_plan.valid) { - ROS_DEBUG_STREAM("pm_.current_plan.planStartTime: " << pm_.current_plan.planStartTime); - ROS_DEBUG_STREAM("timeout2: " << tsStart - pm_.current_plan.planStartTime); - ROS_DEBUG_STREAM("NEGOTIATION_TIMEOUT: " << NEGOTIATION_TIMEOUT); - bool isPlanTimeout = (tsStart - pm_.current_plan.planStartTime) > NEGOTIATION_TIMEOUT; - if(isPlanTimeout) { - pm_.current_plan.valid = false; - ROS_DEBUG_STREAM("The current plan did not receive any response. Abort and change to leader state."); - pm_.current_platoon_state = PlatoonState::LEADER; - ROS_DEBUG_STREAM("Changed the state back to Leader"); - } - } - } - } - - // Task 3 - double desiredJoinGap2 = config_.desiredJoinTimeGap * current_speed_; - double maxJoinGap = std::max(config_.desiredJoinGap, desiredJoinGap2); - double currentGap = pm_.getDistanceToFrontVehicle(); - ROS_DEBUG_STREAM("Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " ms"); - ROS_DEBUG_STREAM("Since we have max allowed gap as " << desiredJoinGap << " m then max join gap became " << maxJoinGap << " m"); - ROS_DEBUG_STREAM("The current gap from radar is " << currentGap << " m"); - // TODO: temporary - if(current_lane_index_ == cf_target_lane_index_ && has_received_join_requirements_) //(currentGap <= maxJoinGap && pm_.current_plan.valid == false) { - { - ROS_DEBUG_STREAM("We are now in the target lane index provided by the leader: " << cf_target_lane_index_); - cav_msgs::MobilityRequest request; - std::string planId = boost::uuids::to_string(boost::uuids::random_generator()()); - long currentTime = ros::Time::now().toNSec()/1000000; - request.m_header.plan_id = planId; - request.m_header.recipient_id = pm_.targetLeaderId; - request.m_header.sender_bsm_id = host_bsm_id_; - request.m_header.sender_id = config_.vehicleID; - request.m_header.timestamp = currentTime; - request.plan_type.type = cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; - request.strategy = MOBILITY_STRATEGY; - request.strategy_params = ""; - request.urgency = 50; - request.location = pose_to_ecef(pose_msg_); - mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Published Mobility Candidate-Join request to the leader"); - ROS_WARN("Published Mobility Candidate-Join request to the leader"); - PlatoonPlan* new_plan = new PlatoonPlan(true, currentTime, planId, pm_.targetLeaderId); - - pm_.current_plan = *new_plan; - - // CandidateFollower is in the target platooning lane; a lane change is not required - cf_lane_change_required_ = false; - } - - //Task 4 - if(pm_.getTotalPlatooningSize() > 1) { - cav_msgs::MobilityOperation status; - status = composeMobilityOperationCandidateFollower(); - mobility_operation_publisher_(status); - ROS_DEBUG_STREAM("Published platoon STATUS operation message"); - } - long tsEnd = ros::Time::now().toNSec()/1000000; - long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration/1000).sleep(); - - } - - MobilityRequestResponse PlatoonStrategicPlugin::handle_mob_req(const cav_msgs::MobilityRequest& msg) - { - MobilityRequestResponse mobility_response; - - - if (pm_.current_platoon_state == PlatoonState::LEADER) - { - mobility_response = mob_req_cb_leader(msg); - } - else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) - { - mobility_response = mob_req_cb_follower(msg); - } - else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) - { - mobility_response = mob_req_cb_candidatefollower(msg); - } - else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) - { - mobility_response = mob_req_cb_leaderwaiting(msg); - } - else if (pm_.current_platoon_state == PlatoonState::STANDBY) - { - mobility_response = mob_req_cb_standby(msg); - } - - return mobility_response; - - } - - void PlatoonStrategicPlugin::mob_req_cb(const cav_msgs::MobilityRequest& msg) - { - cav_msgs::MobilityResponse response; - response.m_header.sender_id = config_.vehicleID; - response.m_header.recipient_id = msg.m_header.sender_id; - response.m_header.plan_id = pm_.currentPlatoonID; - response.m_header.sender_bsm_id = host_bsm_id_; - response.m_header.timestamp = ros::Time::now().toNSec()/1000000; - MobilityRequestResponse req_response = handle_mob_req(msg); - if (req_response == MobilityRequestResponse::ACK) - { - response.is_accepted = true; - mobility_response_publisher_(response); - } - else if (req_response == MobilityRequestResponse::NACK) - { - response.is_accepted = false; - mobility_response_publisher_(response); - } - else - { - ROS_DEBUG_STREAM(" NO response to mobility request. "); - } - } - - MobilityRequestResponse PlatoonStrategicPlugin::mob_req_cb_standby(const cav_msgs::MobilityRequest& msg) - { - // In standby state, the plugin is not responsible for replying to any request messages - return MobilityRequestResponse::NO_RESPONSE; - } - - MobilityRequestResponse PlatoonStrategicPlugin::mob_req_cb_candidatefollower(const cav_msgs::MobilityRequest& msg) - { - // This state does not handle any mobility request for now - // TODO Maybe it should handle some ABORT request from a waiting leader - ROS_DEBUG_STREAM("Recived mobility request with type " << msg.plan_type.type << " but ignored."); - return MobilityRequestResponse::NO_RESPONSE; - - } - - MobilityRequestResponse PlatoonStrategicPlugin::mob_req_cb_follower(const cav_msgs::MobilityRequest& msg) - { - return MobilityRequestResponse::NO_RESPONSE; - } - - MobilityRequestResponse PlatoonStrategicPlugin::mob_req_cb_leaderwaiting(const cav_msgs::MobilityRequest& msg) - { - - bool isTargetVehicle = (msg.m_header.sender_id == lw_applicantId_); - bool isCandidateJoin = msg.plan_type.type == cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; - - lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); - double obj_cross_track = wm_->routeTrackPos(incoming_pose).crosstrack; - bool inTheSameLane = (abs(obj_cross_track - current_crosstrack_) < config_.maxCrosstrackError); - ROS_DEBUG_STREAM("current_cross_track error = " << abs(obj_cross_track - current_crosstrack_)); - ROS_DEBUG_STREAM("inTheSameLane = " << inTheSameLane); - if(isTargetVehicle && isCandidateJoin && inTheSameLane) - { - ROS_DEBUG_STREAM("Target vehicle " << lw_applicantId_ << " is actually joining."); - ROS_DEBUG_STREAM("Changing to PlatoonLeaderState and send ACK to target vehicle"); - pm_.current_platoon_state = PlatoonState::LEADER; - return MobilityRequestResponse::ACK; - } - else - { - ROS_DEBUG_STREAM("Received platoon request with vehicle id = " << msg.m_header.sender_id); - ROS_DEBUG_STREAM("The request type is " << msg.plan_type.type << " and we choose to ignore"); - return MobilityRequestResponse::NO_RESPONSE; - } - - } - - MobilityRequestResponse PlatoonStrategicPlugin::mob_req_cb_leader(const cav_msgs::MobilityRequest& msg) - { - cav_msgs::PlanType plan_type= msg.plan_type; - - // Note: Do not process a 'JOIN_PLATOON_AT_REAR' request if host vehicle is currently - // required to conduct a lane change to form a platoon with another vehicle. - if (plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_AT_REAR && !leader_lane_change_required_) - { - // We are currently checking two basic JOIN conditions: - // 1. The size limitation on current platoon based on the plugin's parameters. - // 2. Calculate how long that vehicle can be in a reasonable distance to actually join us. - // TODO We ignore the lane information for now and assume the applicant is in the same lane with us. - cav_msgs::MobilityHeader msgHeader = msg.m_header; - std::string params = msg.strategy_params; - std::string applicantId = msgHeader.sender_id; - ROS_DEBUG_STREAM("Receive mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id); - ROS_DEBUG_STREAM("The strategy parameters are " << params); - if (params == "") - { - ROS_DEBUG_STREAM("The strategy parameters are empty, return no response"); - return MobilityRequestResponse::NO_RESPONSE; - } - // For JOIN_PLATOON_AT_REAR message, the strategy params is defined as "SIZE:xx,SPEED:xx,DTD:xx" - // TODO In future, we should remove down track distance from this string and use location field in request message - std::vector inputsParams; - boost::algorithm::split(inputsParams, params, boost::is_any_of(",")); - - std::vector applicantSize_parsed; - boost::algorithm::split(applicantSize_parsed, inputsParams[0], boost::is_any_of(":")); - int applicantSize = std::stoi(applicantSize_parsed[1]); - ROS_DEBUG_STREAM("applicantSize: " << applicantSize); - - std::vector applicantCurrentSpeed_parsed; - boost::algorithm::split(applicantCurrentSpeed_parsed, inputsParams[1], boost::is_any_of(":")); - double applicantCurrentSpeed = std::stod(applicantCurrentSpeed_parsed[1]); - ROS_DEBUG_STREAM("applicantCurrentSpeed: " << applicantCurrentSpeed); - - std::vector applicantCurrentDtd_parsed; - boost::algorithm::split(applicantCurrentDtd_parsed, inputsParams[2], boost::is_any_of(":")); - double applicantCurrentDtd = std::stod(applicantCurrentDtd_parsed[1]); - ROS_DEBUG_STREAM("applicantCurrentDtd from message: " << applicantCurrentDtd); - - lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); - applicantCurrentDtd = wm_->routeTrackPos(incoming_pose).downtrack; - ROS_DEBUG_STREAM("applicantCurrentmemberUpdates from ecef pose: " << applicantCurrentDtd); - - double applicant_crosstrack = wm_->routeTrackPos(incoming_pose).crosstrack; - ROS_DEBUG_STREAM("applicant_crosstrack from ecef pose: " << applicant_crosstrack); - bool isInLane = (abs(applicant_crosstrack - current_crosstrack_) < config_.maxCrosstrackError); - ROS_DEBUG_STREAM("isInLane = " << isInLane); - // Check if we have enough room for that applicant - int currentPlatoonSize = pm_.getTotalPlatooningSize(); - bool hasEnoughRoomInPlatoon = applicantSize + currentPlatoonSize <= maxPlatoonSize_; - if(hasEnoughRoomInPlatoon && isInLane) { - ROS_DEBUG_STREAM("The current platoon has enough room for the applicant with size " << applicantSize); - double currentRearDtd = pm_.getPlatoonRearDowntrackDistance(); - ROS_DEBUG_STREAM("The current platoon rear dtd is " << currentRearDtd); - double currentGap = currentRearDtd - applicantCurrentDtd - config_.vehicleLength; - double currentTimeGap = currentGap / applicantCurrentSpeed; - ROS_DEBUG_STREAM("The gap between current platoon rear and applicant is " << currentGap << "m or " << currentTimeGap << "s"); - if(currentGap < 0) { - ROS_WARN("We should not receive any request from the vehicle in front of us. NACK it."); - return MobilityRequestResponse::NACK; - } - // Check if the applicant can join based on max timeGap/gap - bool isDistanceCloseEnough = (currentGap <= maxAllowedJoinGap_) || (currentTimeGap <= maxAllowedJoinTimeGap_); - bool laneConditionsSatisfied = !in_rightmost_lane_ || single_lane_road_; - if(isDistanceCloseEnough && laneConditionsSatisfied) { - ROS_DEBUG_STREAM("The applicant is close enough and we will allow it to try to join"); - ROS_DEBUG_STREAM("Change to LeaderWaitingState and waiting for " << msg.m_header.sender_id << " to join"); - pm_.current_platoon_state = PlatoonState::LEADERWAITING; - waitingStartTime = ros::Time::now().toNSec()/1000000; - lw_applicantId_ = msg.m_header.sender_id; - return MobilityRequestResponse::ACK; - } else if(isDistanceCloseEnough && !laneConditionsSatisfied) { - ROS_DEBUG_STREAM("The applicant is close enough, but we must change into a suitable platooning lane before sending ACK."); - lw_applicantId_ = msg.m_header.sender_id; - - // Set flag to indicate that a lane change into a suitable platooning lane is required prior to sending ACK to applicant - leader_lane_change_required_ = true; - return MobilityRequestResponse::NO_RESPONSE; - } else { - ROS_DEBUG_STREAM("The applicant is too far away from us or not in corret lane. NACK."); - ROS_DEBUG_STREAM("isDistanceCloseEnough" << isDistanceCloseEnough); - ROS_DEBUG_STREAM("laneConditionsSatisfied" << laneConditionsSatisfied); - return MobilityRequestResponse::NACK; - } - } - else - { - ROS_DEBUG_STREAM("The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK"); - return MobilityRequestResponse::NACK; - } - } - else { - ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " and ignored."); - return MobilityRequestResponse::NO_RESPONSE; - } - } - - void PlatoonStrategicPlugin::mob_resp_cb(const cav_msgs::MobilityResponse& msg) - { - if (pm_.current_platoon_state == PlatoonState::LEADER) - { - mob_resp_cb_leader(msg); - } - else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) - { - mob_resp_cb_follower(msg); - } - else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) - { - mob_resp_cb_candidatefollower(msg); - } - else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) - { - mob_resp_cb_leaderwaiting(msg); - } - else if (pm_.current_platoon_state == PlatoonState::STANDBY) - { - mob_resp_cb_standby(msg); - } - } - - void PlatoonStrategicPlugin::mob_resp_cb_standby(const cav_msgs::MobilityResponse& msg) - { - // In standby state, it will not send out any requests so it will also ignore all responses - } - - - void PlatoonStrategicPlugin::mob_resp_cb_candidatefollower(const cav_msgs::MobilityResponse& msg) - { - ROS_DEBUG_STREAM("Callback for candidate follower "); - if (pm_.current_plan.valid) - { - bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId; - bool isFromTargetVehicle = msg.m_header.sender_id == pm_.targetLeaderId; - ROS_DEBUG_STREAM("isForCurrentPlan " << isForCurrentPlan); - - ROS_DEBUG_STREAM("isFromTargetVehicle " << isFromTargetVehicle); - - //if (isForCurrentPlan && isFromTargetVehicle) //TODO: This check not needed for now - if (true) - { - if(msg.is_accepted) - { - // We change to follower state and start to actually follow that leader - // The platoon manager also need to change the platoon Id to the one that the target leader is using - ROS_DEBUG_STREAM("The leader " << msg.m_header.sender_id << " agreed on our join. Change to follower state."); - pm_.current_platoon_state = PlatoonState::FOLLOWER; - targetPlatoonId = msg.m_header.plan_id; - pm_.changeFromLeaderToFollower(targetPlatoonId); - ROS_WARN("changed to follower"); - - } - else - { - // We change back to normal leader state and try to join other platoons - ROS_DEBUG_STREAM("The leader " << msg.m_header.sender_id << " does not agree on our join. Change back to leader state."); - pm_.current_platoon_state = PlatoonState::LEADER; - } - } - else - { - ROS_DEBUG_STREAM("Ignore received response message because it is not for the current plan."); - } - } - else - { - ROS_DEBUG_STREAM("Ignore received response message because we are not in any negotiation process."); - } - } - - void PlatoonStrategicPlugin::mob_resp_cb_leaderwaiting(const cav_msgs::MobilityResponse& msg) - { - } - - void PlatoonStrategicPlugin::mob_resp_cb_follower(const cav_msgs::MobilityResponse& msg) - { - } - - void PlatoonStrategicPlugin::mob_resp_cb_leader(const cav_msgs::MobilityResponse& msg) - { - if (pm_.current_plan.valid) - { - // if (pm_.current_plan.planId == msg.m_header.plan_id && pm_.current_plan.peerId == msg.m_header.sender_id) //TODO this check not needed here, - if (true) - { - if (msg.is_accepted) - { - ROS_DEBUG_STREAM("Received positive response for plan id = " << pm_.current_plan.planId); - ROS_DEBUG_STREAM("Change to CandidateFollower state and notify trajectory failure in order to replan"); - // Change to candidate follower state and request a new plan to catch up with the front platoon - pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; - has_received_join_requirements_ = false; - candidatestateStartTime = ros::Time::now().toNSec()/1000000; - targetPlatoonId = potentialNewPlatoonId; - ROS_DEBUG_STREAM("targetPlatoonId = " << targetPlatoonId); - pm_.targetLeaderId = pm_.current_plan.peerId; - ROS_DEBUG_STREAM("pm_.targetLeaderId = " << pm_.targetLeaderId ); - } - else - { - ROS_DEBUG_STREAM("Received negative response for plan id = " << pm_.current_plan.planId); - // Forget about the previous plan totally - pm_.current_plan.valid = false; - } - } - else - { - ROS_DEBUG_STREAM("Ignore the response message because planID match: " << (pm_.current_plan.planId == msg.m_header.plan_id)); - ROS_DEBUG_STREAM("My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id); - ROS_DEBUG_STREAM("And peer id match " << (pm_.current_plan.peerId == msg.m_header.sender_id)); - ROS_DEBUG_STREAM("Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id); - } - } - } - - cav_msgs::PlatooningInfo PlatoonStrategicPlugin::composePlatoonInfoMsg() - { - cav_msgs::PlatooningInfo status_msg; - - if (pm_.current_platoon_state == PlatoonState::STANDBY) - { - status_msg.state = cav_msgs::PlatooningInfo::DISABLED; - } - else if (pm_.current_platoon_state == PlatoonState::LEADER) - { - status_msg.state = pm_.getTotalPlatooningSize() == 1 ? cav_msgs::PlatooningInfo::SEARCHING : cav_msgs::PlatooningInfo::LEADING; - } - else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) - { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; - } - else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) - { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_LEADER; - } - else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) - { - status_msg.state = cav_msgs::PlatooningInfo::FOLLOWING; - } - - if (pm_.current_platoon_state != PlatoonState::STANDBY) - { - status_msg.platoon_id = pm_.currentPlatoonID; - status_msg.size = std::max(1, pm_.getTotalPlatooningSize()); - status_msg.size_limit = config_.maxPlatoonSize; - - - - if (pm_.current_platoon_state == PlatoonState::FOLLOWER) - { - ROS_DEBUG_STREAM("isFollower: " << pm_.isFollower); - ROS_DEBUG_STREAM("pm platoonsize: " << pm_.platoon.size()); - - pm_.isFollower = true; - - PlatoonMember platoon_leader = pm_.getLeader(); - ROS_DEBUG_STREAM("platoon_leader " << platoon_leader.staticId); - status_msg.leader_id = platoon_leader.staticId; - status_msg.leader_downtrack_distance = platoon_leader.vehiclePosition; - ROS_DEBUG_STREAM("platoon_leader position: " << platoon_leader.vehiclePosition); - status_msg.leader_cmd_speed = platoon_leader.commandSpeed; - status_msg.host_platoon_position = pm_.getNumberOfVehicleInFront(); - - status_msg.desired_gap = std::max(config_.standStillHeadway, config_.timeHeadway * current_speed_); - status_msg.actual_gap = platoon_leader.vehiclePosition - current_downtrack_; - - } - else - { - status_msg.leader_id = config_.vehicleID; - status_msg.leader_downtrack_distance = current_downtrack_; - status_msg.leader_cmd_speed = cmd_speed_; - status_msg.host_platoon_position = 0; - - } - - // This info is updated at platoon control plugin - status_msg.host_cmd_speed = cmd_speed_; - - - } - return status_msg; - } - - void PlatoonStrategicPlugin::mob_op_cb(const cav_msgs::MobilityOperation& msg) - { - if (pm_.current_platoon_state == PlatoonState::LEADER) - { - mob_op_cb_leader(msg); - } - else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) - { - mob_op_cb_follower(msg); - } - else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) - { - mob_op_cb_candidatefollower(msg); - } - else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) - { - mob_op_cb_leaderwaiting(msg); - } - else if (pm_.current_platoon_state == PlatoonState::STANDBY) - { - mob_op_cb_standby(msg); - } - - - // TODO: If needed (with large size platoons), add a queue for status messages - // INFO messages always processed, STATUS messages if saved in que - - } - - void PlatoonStrategicPlugin::mob_op_cb_standby(const cav_msgs::MobilityOperation& msg) - { - // In standby state, it will ignore operation message since it is not actively operating - } - - void PlatoonStrategicPlugin::mob_op_cb_candidatefollower(const cav_msgs::MobilityOperation& msg) - { - // We still need to handle STATUS operAtion message from our platoon - std::string strategyParams = msg.strategy_params; - bool isPlatoonStatusMsg = (strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0); - bool isJoinRequirementsMsg = (strategyParams.rfind(OPERATION_JOIN_REQUIREMENTS_TYPE, 0) == 0); - - if(isPlatoonStatusMsg) { - std::string vehicleID = msg.m_header.sender_id; - std::string platoonId = msg.m_header.plan_id; - std::string statusParams = strategyParams.substr(OPERATION_STATUS_TYPE.size() + 1); - - std::vector inputsParams; - boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); - - std::vector ecef_x_parsed; - boost::algorithm::split(ecef_x_parsed, inputsParams[3], boost::is_any_of(":")); - double ecef_x = std::stod(ecef_x_parsed[1]); - ROS_DEBUG_STREAM("ecef_x_parsed: " << ecef_x); - - std::vector ecef_y_parsed; - boost::algorithm::split(ecef_y_parsed, inputsParams[4], boost::is_any_of(":")); - double ecef_y = std::stod(ecef_y_parsed[1]); - ROS_DEBUG_STREAM("ecef_y_parsed: " << ecef_y); - - std::vector ecef_z_parsed; - boost::algorithm::split(ecef_z_parsed, inputsParams[5], boost::is_any_of(":")); - double ecef_z = std::stod(ecef_z_parsed[1]); - ROS_DEBUG_STREAM("ecef_z_parsed: " << ecef_z); - - cav_msgs::LocationECEF ecef_loc; - ecef_loc.ecef_x = ecef_x; - ecef_loc.ecef_y = ecef_y; - ecef_loc.ecef_z = ecef_z; - - lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); - double dtd = wm_->routeTrackPos(incoming_pose).downtrack; - - pm_.memberUpdates(vehicleID, platoonId, msg.m_header.sender_bsm_id, statusParams, dtd); - ROS_DEBUG_STREAM("Received platoon status message from " << msg.m_header.sender_id); - } - else if(isJoinRequirementsMsg) { - bool isForHostVehicle = msg.m_header.recipient_id == config_.vehicleID; - bool isFromTargetLeader = msg.m_header.sender_id == pm_.targetLeaderId; - - if (isForHostVehicle && isFromTargetLeader) { - has_received_join_requirements_ = true; - - // JOIN_REQUIREMENTS message uses params string format "JOIN_REQUIREMENTS|LANE_INDEX:xx,LANE_GROUP_SIZE:xx" - std::vector inputsParams; - boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); - - std::vector target_lane_index_parsed; - boost::algorithm::split(target_lane_index_parsed, inputsParams[0], boost::is_any_of(":")); - int target_lane_index = std::stoi(target_lane_index_parsed[1]); - - std::vector target_lane_group_size_parsed; - boost::algorithm::split(target_lane_group_size_parsed, inputsParams[1], boost::is_any_of(":")); - int target_lane_group_size = std::stoi(target_lane_group_size_parsed[1]); - - ROS_WARN_STREAM("Received JOIN_REQUIREMENTS MobilityOperation with target lane index: " << target_lane_index \ - << " and lane group size " << target_lane_group_size); - - // Note: If the target Leader vehicle is located on a different lanelet/lane group than the host vehicle, and the two lane groups - // have a different quantity of lanes in the current travel direction, then the communication of 'lane index' may - // be error-prone (i.e. a lane index of '1' may refer to a different lane for both vehicles). This is a known edge - // case that this plugin does not currently cover. - if (target_lane_group_size != current_lane_group_size_) { - ROS_WARN_STREAM("Target leader's lane group size is " << target_lane_group_size << ", host vehicle's is " \ - << current_lane_group_size_ << ". Lane-index communication may be incorrect."); - } - - // Store the target CandidateFollower platoon lane index provided by the target Leader - cf_target_lane_index_ = target_lane_index; - - // Set flag to indicate CandidateFollower must change lanes if it is not currently in the target lane platoon lane index - if (current_lane_index_ != cf_target_lane_index_) { - cf_lane_change_required_ = true; - } - } - } - else { - ROS_DEBUG_STREAM("Received a mobility operation message with params " << msg.strategy_params << " but ignored."); - } - } - - void PlatoonStrategicPlugin::mob_op_cb_follower(const cav_msgs::MobilityOperation& msg) - { - std::string strategyParams = msg.strategy_params; - // In the current state, we care about the STATUS message - bool isPlatoonStatusMsg = (strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0); - bool isPlatoonInfoMsg = (strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0); - // If it is platoon status message, the params string is in format: - // STATUS|CMDSPEED:xx,DTD:xx,SPEED:xx - if(isPlatoonStatusMsg) { - std::string vehicleID = msg.m_header.sender_id; - std::string platoonID = msg.m_header.plan_id; - std::string senderBSM = msg.m_header.sender_bsm_id; - std::string statusParams = strategyParams.substr(OPERATION_STATUS_TYPE.size() + 1); - ROS_DEBUG_STREAM("Receive operation message from vehicle: " << vehicleID); - - std::vector inputsParams; - boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); - - std::vector ecef_x_parsed; - boost::algorithm::split(ecef_x_parsed, inputsParams[3], boost::is_any_of(":")); - double ecef_x = std::stod(ecef_x_parsed[1]); - ROS_DEBUG_STREAM("ecef_x_parsed: " << ecef_x); - - std::vector ecef_y_parsed; - boost::algorithm::split(ecef_y_parsed, inputsParams[4], boost::is_any_of(":")); - double ecef_y = std::stod(ecef_y_parsed[1]); - ROS_DEBUG_STREAM("ecef_y_parsed: " << ecef_y); - - std::vector ecef_z_parsed; - boost::algorithm::split(ecef_z_parsed, inputsParams[5], boost::is_any_of(":")); - double ecef_z = std::stod(ecef_z_parsed[1]); - ROS_DEBUG_STREAM("ecef_z_parsed: " << ecef_z); - - cav_msgs::LocationECEF ecef_loc; - ecef_loc.ecef_x = ecef_x; - ecef_loc.ecef_y = ecef_y; - ecef_loc.ecef_z = ecef_z; - - lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); - double dtd = wm_->routeTrackPos(incoming_pose).downtrack; - ROS_DEBUG_STREAM("DTD calculated in mob_op_cb_follower: " << dtd); - - pm_.memberUpdates(vehicleID, platoonID, senderBSM, statusParams, dtd); - } - } - - void PlatoonStrategicPlugin::mob_op_cb_leaderwaiting(const cav_msgs::MobilityOperation& msg) - { - std::string strategyParams = msg.strategy_params; - bool isPlatoonStatusMsg = (strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0); - if(isPlatoonStatusMsg) { - std::string vehicleID = msg.m_header.sender_id; - std::string platoonId = msg.m_header.plan_id; - std::string statusParams = strategyParams.substr(OPERATION_STATUS_TYPE.size() + 1); - - - std::vector inputsParams; - boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); - - std::vector ecef_x_parsed; - boost::algorithm::split(ecef_x_parsed, inputsParams[3], boost::is_any_of(":")); - double ecef_x = std::stod(ecef_x_parsed[1]); - ROS_DEBUG_STREAM("ecef_x_parsed: " << ecef_x); - - std::vector ecef_y_parsed; - boost::algorithm::split(ecef_y_parsed, inputsParams[4], boost::is_any_of(":")); - double ecef_y = std::stod(ecef_y_parsed[1]); - ROS_DEBUG_STREAM("ecef_y_parsed: " << ecef_y); - - std::vector ecef_z_parsed; - boost::algorithm::split(ecef_z_parsed, inputsParams[5], boost::is_any_of(":")); - double ecef_z = std::stod(ecef_z_parsed[1]); - ROS_DEBUG_STREAM("ecef_z_parsed: " << ecef_z); - - cav_msgs::LocationECEF ecef_loc; - ecef_loc.ecef_x = ecef_x; - ecef_loc.ecef_y = ecef_y; - ecef_loc.ecef_z = ecef_z; - - lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); - double dtd = wm_->routeTrackPos(incoming_pose).downtrack; - - pm_.memberUpdates(vehicleID, platoonId, msg.m_header.sender_bsm_id, statusParams, dtd); - ROS_DEBUG_STREAM("Received platoon status message from " << msg.m_header.sender_id); - ROS_DEBUG_STREAM("member updated"); - } else { - ROS_DEBUG_STREAM("Received a mobility operation message with params " << msg.strategy_params << " but ignored."); - } - } - - void PlatoonStrategicPlugin::mob_op_cb_leader(const cav_msgs::MobilityOperation& msg) - { - std::string strategyParams = msg.strategy_params; - std::string senderId = msg.m_header.sender_id; - std::string platoonId = msg.m_header.plan_id; - // In the current state, we care about the INFO heart-beat operation message if we are not currently in - // a negotiation, and also we need to care about operation from members in our current platoon - - bool isPlatoonInfoMsg = (strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0); - bool isPlatoonStatusMsg = (strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0); - bool isNotInNegotiation = (pm_.current_plan.valid == false); - if(isPlatoonInfoMsg && isNotInNegotiation) - { - // For INFO params, the string format is INFO|REAR:%s,LENGTH:%.2f,SPEED:%.2f,SIZE:%d,DTD:%.2f - - std::vector inputsParams; - boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); - - std::vector rearVehicleBsmId_parsed; - boost::algorithm::split(rearVehicleBsmId_parsed, inputsParams[0], boost::is_any_of(":")); - std::string rearVehicleBsmId = rearVehicleBsmId_parsed[1]; - ROS_DEBUG_STREAM("rearVehicleBsmId: " << rearVehicleBsmId); - - std::vector rearVehicleDtd_parsed; - boost::algorithm::split(rearVehicleDtd_parsed, inputsParams[4], boost::is_any_of(":")); - double rearVehicleDtd = std::stod(rearVehicleDtd_parsed[1]); - - - ROS_DEBUG_STREAM("rearVehicleDtd from message: " << rearVehicleDtd); - - std::vector ecef_x_parsed; - boost::algorithm::split(ecef_x_parsed, inputsParams[5], boost::is_any_of(":")); - double ecef_x = std::stod(ecef_x_parsed[1]); - ROS_DEBUG_STREAM("ecef_x_parsed: " << ecef_x); - - std::vector ecef_y_parsed; - boost::algorithm::split(ecef_y_parsed, inputsParams[6], boost::is_any_of(":")); - double ecef_y = std::stod(ecef_y_parsed[1]); - ROS_DEBUG_STREAM("ecef_y_parsed: " << ecef_y); - - std::vector ecef_z_parsed; - boost::algorithm::split(ecef_z_parsed, inputsParams[7], boost::is_any_of(":")); - double ecef_z = std::stod(ecef_z_parsed[1]); - ROS_DEBUG_STREAM("ecef_z_parsed: " << ecef_z); - - cav_msgs::LocationECEF ecef_loc; - ecef_loc.ecef_x = ecef_x; - ecef_loc.ecef_y = ecef_y; - ecef_loc.ecef_z = ecef_z; - - lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); - rearVehicleDtd = wm_->routeTrackPos(incoming_pose).downtrack; - - ROS_DEBUG_STREAM("rearVehicleDtd from ecef: " << rearVehicleDtd); - - // We are trying to validate is the platoon rear is right in front of the host vehicle - if(isVehicleRightInFront(rearVehicleBsmId, rearVehicleDtd)) - { - ROS_DEBUG_STREAM("Found a platoon with id = " << platoonId << " in front of us."); - cav_msgs::MobilityRequest request; - request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); - request.m_header.recipient_id = senderId; - request.m_header.sender_bsm_id = host_bsm_id_; - request.m_header.sender_id = config_.vehicleID; - request.m_header.timestamp = ros::Time::now().toNSec()/1000000; - request.location = pose_to_ecef(pose_msg_); - request.plan_type.type = cav_msgs::PlanType::JOIN_PLATOON_AT_REAR; - request.strategy = MOBILITY_STRATEGY; - - int platoon_size = pm_.getTotalPlatooningSize(); - - - boost::format fmter(JOIN_AT_REAR_PARAMS); - fmter %platoon_size; - fmter %current_speed_; - fmter %current_downtrack_; - fmter %pose_ecef_point_.ecef_x; - fmter %pose_ecef_point_.ecef_y; - fmter %pose_ecef_point_.ecef_z; - - request.strategy_params = fmter.str(); - request.urgency = 50; - - pm_.current_plan = PlatoonPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); - mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Publishing request to leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); - potentialNewPlatoonId = platoonId; - } - else - { - ROS_DEBUG_STREAM("Ignore platoon with platoon id: " << platoonId << " because it is not right in front of us"); - } - } - else if(isPlatoonStatusMsg) - { - // If it is platoon status message, the params string is in format: STATUS|CMDSPEED:xx,DTD:xx,SPEED:xx - std::string statusParams = strategyParams.substr(OPERATION_STATUS_TYPE.length() + 1); - ROS_DEBUG_STREAM("Receive operation status message from vehicle: " << senderId << " with params: " << statusParams); - - std::vector inputsParams; - boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); - - std::vector ecef_x_parsed; - boost::algorithm::split(ecef_x_parsed, inputsParams[3], boost::is_any_of(":")); - double ecef_x = std::stod(ecef_x_parsed[1]); - ROS_DEBUG_STREAM("ecef_x_parsed: " << ecef_x); - - std::vector ecef_y_parsed; - boost::algorithm::split(ecef_y_parsed, inputsParams[4], boost::is_any_of(":")); - double ecef_y = std::stod(ecef_y_parsed[1]); - ROS_DEBUG_STREAM("ecef_y_parsed: " << ecef_y); - - std::vector ecef_z_parsed; - boost::algorithm::split(ecef_z_parsed, inputsParams[5], boost::is_any_of(":")); - double ecef_z = std::stod(ecef_z_parsed[1]); - ROS_DEBUG_STREAM("ecef_z_parsed: " << ecef_z); - - cav_msgs::LocationECEF ecef_loc; - ecef_loc.ecef_x = ecef_x; - ecef_loc.ecef_y = ecef_y; - ecef_loc.ecef_z = ecef_z; - - lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); - - double dtd = wm_->routeTrackPos(incoming_pose).downtrack; - ROS_DEBUG_STREAM("dtd from ecef: " << dtd); - pm_.memberUpdates(senderId, platoonId, msg.m_header.sender_bsm_id, statusParams, dtd); - - } - else - { - ROS_DEBUG_STREAM("Receive operation message but ignore it because isPlatoonInfoMsg = " << isPlatoonInfoMsg << - ", isNotInNegotiation = " << isNotInNegotiation << " and isPlatoonStatusMsg = " << isPlatoonStatusMsg); - } - } - - bool PlatoonStrategicPlugin::isVehicleRightInFront(std::string rearVehicleBsmId, double downtrack) { - double currentDtd = current_downtrack_; - if(downtrack > currentDtd) { - ROS_DEBUG_STREAM("Found a platoon in front. We are able to join"); - return true; - } else { - ROS_DEBUG_STREAM("Ignoring platoon from our back."); - ROS_DEBUG_STREAM("The front platoon dtd is " << downtrack << " and we are current at " << currentDtd); - return false; - } - } - - cav_msgs::LocationECEF PlatoonStrategicPlugin::pose_to_ecef(geometry_msgs::PoseStamped pose_msg) - { - - if (!map_projector_) { - throw std::invalid_argument("No map projector available for ecef conversion"); - } - - cav_msgs::LocationECEF location; - - lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({pose_msg.pose.position.x, pose_msg.pose.position.y, 0.0}, 1); - location.ecef_x = ecef_point.x() * 100.0; - location.ecef_y = ecef_point.y() * 100.0; - location.ecef_z = ecef_point.z() * 100.0; - - - ROS_DEBUG_STREAM("location.ecef_x: " << location.ecef_x); - ROS_DEBUG_STREAM("location.ecef_y: " << location.ecef_y); - ROS_DEBUG_STREAM("location.ecef_z: " << location.ecef_z); - - return location; - } - - lanelet::BasicPoint2d PlatoonStrategicPlugin::ecef_to_map_point(cav_msgs::LocationECEF ecef_point) - { - if (!map_projector_) { - throw std::invalid_argument("No map projector available for ecef conversion"); - } - - lanelet::BasicPoint3d map_point = map_projector_->projectECEF( { (double)ecef_point.ecef_x/100.0, (double)ecef_point.ecef_y/100.0, (double)ecef_point.ecef_z/100.0 } , -1); - - lanelet::BasicPoint2d output {map_point.x(), map_point.y()}; - - ROS_DEBUG_STREAM("map_point.x(): " << map_point.x()); - ROS_DEBUG_STREAM("map_point.y(): " << map_point.y()); - return output; - } - - void PlatoonStrategicPlugin::georeference_cb(const std_msgs::StringConstPtr& msg) - { - map_projector_ = std::make_shared(msg->data.c_str()); // Build projector from proj string - } - - - cav_msgs::MobilityOperation PlatoonStrategicPlugin::composeMobilityOperationLeader(const std::string& type){ - cav_msgs::MobilityOperation msg; - msg.m_header.plan_id = pm_.currentPlatoonID; - msg.m_header.recipient_id = ""; - msg.m_header.sender_bsm_id = host_bsm_id_; - std::string hostStaticId = config_.vehicleID; - msg.m_header.sender_id = hostStaticId; - msg.m_header.timestamp = ros::Time::now().toNSec()/1000000;; - msg.strategy = MOBILITY_STRATEGY; - - if (type == OPERATION_INFO_TYPE){ - // For INFO params, the string format is INFO|REAR:%s,LENGTH:%.2f,SPEED:%.2f,SIZE:%d,DTD:%.2f - - std::string PlatoonRearBsmId = host_bsm_id_; - int CurrentPlatoonLength = pm_.getCurrentPlatoonLength(); - double current_speed = current_speed_; - int PlatoonSize = pm_.getTotalPlatooningSize(); - double PlatoonRearDowntrackDistance = pm_.getPlatoonRearDowntrackDistance(); - - boost::format fmter(OPERATION_INFO_PARAMS); - fmter %PlatoonRearBsmId; - fmter %CurrentPlatoonLength; - fmter %current_speed; - fmter %PlatoonSize; - fmter %PlatoonRearDowntrackDistance; - fmter %pose_ecef_point_.ecef_x; - fmter %pose_ecef_point_.ecef_y; - fmter %pose_ecef_point_.ecef_z; - - std::string infoParams = fmter.str(); - msg.strategy_params = infoParams; - } - else if (type == OPERATION_STATUS_TYPE){ - // For STATUS params, the string format is "STATUS|CMDSPEED:xx,DTD:xx,SPEED:xx" - double cmdSpeed = cmd_speed_; - boost::format fmter(OPERATION_STATUS_PARAMS); - fmter %cmdSpeed; - fmter %current_downtrack_; - fmter %current_speed_; - fmter %pose_ecef_point_.ecef_x; - fmter %pose_ecef_point_.ecef_y; - fmter %pose_ecef_point_.ecef_z; - - std::string statusParams = fmter.str(); - msg.strategy_params = statusParams; - } else { - ROS_ERROR("UNKNOW strategy param string!!!"); - msg.strategy_params = ""; - } - ROS_DEBUG_STREAM("Composed a mobility operation message with params " << msg.strategy_params); - - return msg; - } - - cav_msgs::MobilityOperation PlatoonStrategicPlugin::composeMobilityOperationFollower() - { - cav_msgs::MobilityOperation msg; - msg.m_header.plan_id = pm_.currentPlatoonID; - // All platoon mobility operation message is just for broadcast - msg.m_header.recipient_id = ""; - msg.m_header.sender_bsm_id = host_bsm_id_; - std::string hostStaticId = config_.vehicleID; - msg.m_header.sender_id = hostStaticId; - msg.m_header.timestamp = ros::Time::now().toNSec()/1000000; - msg.strategy = MOBILITY_STRATEGY; - - double cmdSpeed = cmd_speed_; - boost::format fmter(OPERATION_STATUS_PARAMS); - fmter %cmdSpeed; - fmter %current_downtrack_; - fmter %current_speed_; - fmter %pose_ecef_point_.ecef_x; - fmter %pose_ecef_point_.ecef_y; - fmter %pose_ecef_point_.ecef_z; - - std::string statusParams = fmter.str(); - msg.strategy_params = statusParams; - ROS_DEBUG_STREAM("Composed a mobility operation message with params " << msg.strategy_params); - return msg; - } - - - cav_msgs::MobilityOperation PlatoonStrategicPlugin::composeMobilityOperationLeaderWaiting(const std::string& type) - { - cav_msgs::MobilityOperation msg; - msg.m_header.plan_id = pm_.currentPlatoonID; - // This message is for broadcast - msg.m_header.recipient_id = ""; - msg.m_header.sender_bsm_id = host_bsm_id_; - std::string hostStaticId = config_.vehicleID; - msg.m_header.sender_id = hostStaticId; - msg.m_header.timestamp = ros::Time::now().toNSec()/1000000; - - msg.strategy = MOBILITY_STRATEGY; - - if (type == OPERATION_STATUS_TYPE){ - // For STATUS params, the string format is "STATUS|CMDSPEED:5.0,DOWNTRACK:100.0,SPEED:5.0" - - double cmdSpeed = cmd_speed_; - boost::format fmter(OPERATION_STATUS_PARAMS); - fmter %cmdSpeed; - fmter %current_downtrack_; - fmter %current_speed_; - fmter %pose_ecef_point_.ecef_x; - fmter %pose_ecef_point_.ecef_y; - fmter %pose_ecef_point_.ecef_z; - - std::string statusParams = fmter.str(); - msg.strategy_params = statusParams; - } - else if (type == OPERATION_JOIN_REQUIREMENTS_TYPE) { - // Note: If the host vehicle is located on a different lanelet/lane group than the rear vehicle, and the two lane groups - // have a different quantity of lanes in the current travel direction, then the communication of 'lane index' may - // be error-prone (i.e. a lane index of '1' may refer to a different lane for both vehicles). This is a known edge - // case that this plugin does not currently cover. - - msg.m_header.recipient_id = lw_applicantId_; // JOIN_REQUIREMENTS message is intended only for the current applicant - - // For JOIN_REQUIREMENTS params, the string format is "JOIN_REQUIREMENTS|LANE_INDEX:xx,LANE_GROUP_SIZE:xx" - boost::format fmter(OPERATION_JOIN_REQUIREMENTS_PARAMS); - fmter %current_lane_index_; // Local lane index of host vehicle (0 is rightmost, 1 is second rightmost, etc.); considers only the current travel direction - fmter %current_lane_group_size_; // Current quantity of lanes in the host vehicle's local lane group; considers only the current travel direction - - std::string join_params = fmter.str(); - msg.strategy_params = join_params; - - ROS_DEBUG_STREAM("Composed a JOIN_REQUIREMENTS MobilityOperation message with params " << msg.strategy_params); - } - return msg; - } - - cav_msgs::MobilityOperation PlatoonStrategicPlugin::composeMobilityOperationCandidateFollower() - { - cav_msgs::MobilityOperation msg; - - msg.m_header.plan_id = pm_.currentPlatoonID; - // All platoon mobility operation message is just for broadcast - msg.m_header.recipient_id = ""; - msg.m_header.sender_bsm_id = host_bsm_id_; - std::string hostStaticId = config_.vehicleID; - msg.m_header.sender_id = hostStaticId; - msg.m_header.timestamp = ros::Time::now().toNSec()/1000000; - msg.strategy = MOBILITY_STRATEGY; - - // For STATUS params, the string format is "STATUS|CMDSPEED:xx,DTD:xx,SPEED:xx" - double cmdSpeed = cmd_speed_; - boost::format fmter(OPERATION_STATUS_PARAMS); - fmter %cmdSpeed; - fmter %current_downtrack_; - fmter %current_speed_; - fmter %pose_ecef_point_.ecef_x; - fmter %pose_ecef_point_.ecef_y; - fmter %pose_ecef_point_.ecef_z; - - std::string statusParams = fmter.str(); - msg.strategy_params = statusParams; - ROS_DEBUG_STREAM("Composed a mobility operation message with params " << msg.strategy_params); - - return msg; - } - - -} diff --git a/platooning_strategic/test/main_test.cpp b/platooning_strategic/test/main_test.cpp deleted file mode 100644 index 81d202fa20..0000000000 --- a/platooning_strategic/test/main_test.cpp +++ /dev/null @@ -1,27 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-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 - -// Run all the tests -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/platooning_strategic/test/mobility_messages.cpp b/platooning_strategic/test/mobility_messages.cpp deleted file mode 100644 index d6ceeb1529..0000000000 --- a/platooning_strategic/test/mobility_messages.cpp +++ /dev/null @@ -1,277 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-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 -using namespace std; - -class MobilityMessages{ - - private: - cav_msgs::MobilityRequest req1; - cav_msgs::MobilityRequest req2; - cav_msgs::MobilityRequest req3; - cav_msgs::MobilityResponse res1; - cav_msgs::MobilityResponse res2; - cav_msgs::MobilityResponse res3; - cav_msgs::MobilityOperation op1; - cav_msgs::MobilityOperation op2; - cav_msgs::MobilityOperation op3; - - public: - MobilityMessages(){ - cav_msgs::MobilityHeader header; - cav_msgs::PlanType plan; - cav_msgs::LocationECEF location; - cav_msgs::LocationOffsetECEF offset; - cav_msgs::Trajectory traject; - vector offsets; - //mobility request messages - - //set up location - location.ecef_x = 10000; - location.ecef_y = 0; - location.ecef_z = 33333; - location.timestamp = 99999; - - - //set up trajectory - traject.location = location; - offset.offset_x = 14; - offset.offset_y = 200; - offset.offset_z = 150; - offsets.push_back(offset); - traject.offsets = offsets; - - //set up plan - plan.type = 1; - - //set up header - header.sender_id = "sender_id1"; - header.sender_bsm_id = "sender_bsm_id1"; - header.recipient_id = ""; - header.plan_id = "plan_id1"; - header.timestamp = 55555555555555; - - //set up mobility request - req1.m_header = header; - req1.strategy = "CARMA/platooning"; - req1.plan_type = plan; - req1.urgency = 1000; - req1.location = location; - req1.strategy_params = "param1 param2"; - req1.trajectory = traject; - req1.expiration = 5555555; - - - //set up location - location.ecef_x = 40; - location.ecef_y = 12312321; - location.ecef_z = 3177; - location.timestamp = 191934; - - - //set up trajectory - traject.location = location; - offset.offset_x = 189; - offset.offset_y = 2123124; - offset.offset_z = 14; - offsets.push_back(offset); - traject.offsets = offsets; - - //set up plan - plan.type = 2; - - //set up header - header.sender_id = "11"; - header.sender_bsm_id = "sender_bsm_id5"; - header.recipient_id = "recipient_id5"; - header.plan_id = "plan_id5"; - header.timestamp = 555555555555; - - //set up mobility request - req2.m_header = header; - req2.strategy = "CARMA/platooning"; - req2.plan_type.type = cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; - req2.urgency = 1000; - req2.location = location; - req2.strategy_params = "param1 param2 param3 param4"; - req2.trajectory = traject; - req2.expiration = 98124172; - - - //set up location - location.ecef_x = 1238123; - location.ecef_y = 9875488; - location.ecef_z = 3434534; - location.timestamp = 88888888; - - - //set up trajectory - traject.location = location; - offset.offset_x = 987893934; - offset.offset_y = 5151; - offset.offset_z = 12312; - offsets.push_back(offset); - traject.offsets = offsets; - - //set up plan - plan.type = 3; - - //set up header - header.sender_id = "sender_id6"; - header.sender_bsm_id = "sender_bsm_id6"; - header.recipient_id = "recipient_id6"; - header.plan_id = "plan_id6"; - header.timestamp = 98745745; - - //set up mobility request - req3.m_header = header; - req3.strategy = "CARMA/platooning3"; - req3.plan_type = plan; - req3.urgency = 156; - req3.location = location; - req3.strategy_params = "param1 param2"; - req3.trajectory = traject; - req3.expiration = 15132; - - - - //mobility response messages - - - //set up header - header.sender_id = "sender_id2"; - header.sender_bsm_id = "sender_bsm_id2"; - header.recipient_id = "recipient_id1"; - header.plan_id = "plan_id2"; - header.timestamp = 66666666; - - //set up mobility response - res1.m_header = header; - res1.is_accepted = true; - res1.urgency = 1000; - - //set up header - header.sender_id = "sender_id9"; - header.sender_bsm_id = "sender_bsm_id9"; - header.recipient_id = "recipient_id9"; - header.plan_id = "plan_id9"; - header.timestamp = 1772327; - - //set up mobility response - res2.m_header = header; - res2.is_accepted = false; - res2.urgency = 50; - - //set up header - header.sender_id = "sender_id10"; - header.sender_bsm_id = "sender_bsm_id10"; - header.recipient_id = "recipient_id10"; - header.plan_id = "plan_id10"; - header.timestamp = 77777777; - - //set up mobility response - res3.m_header = header; - res3.is_accepted = true; - res3.urgency = 700; - - - - - //mobility operation messages - - //set up header - header.sender_id = "sender_id3"; - header.sender_bsm_id = "sender_bsm_id3"; - header.recipient_id = "recipient_id3"; - header.plan_id = "plan_id3"; - header.timestamp = 44444444; - - op1.m_header = header; - op1.strategy = "STATUS"; - op1.strategy_params = "CMDSPEED:1.0,DTD:1.0,SPEED:1.0"; - - //set up header - header.sender_id = "sender_id12"; - header.sender_bsm_id = "sender_bsm_id12"; - header.recipient_id = "recipient_id12"; - header.plan_id = "plan_id12"; - header.timestamp = 8989898; - - op2.m_header = header; - op2.strategy = "strategy3"; - op2.strategy_params = "param1 param2 param3 param4 param5"; - - - //set up header - header.sender_id = "sender_id23"; - header.sender_bsm_id = "sender_bsm_id23"; - header.recipient_id = "recipient_id23"; - header.plan_id = "plan_id23"; - header.timestamp = 181818; - - op3.m_header = header; - op3.strategy = "strategy23"; - op3.strategy_params = ""; - - } - - cav_msgs::MobilityRequest getRequest1(){ - return req1; - } - - cav_msgs::MobilityRequest getRequest2(){ - return req2; - } - - cav_msgs::MobilityRequest getRequest3(){ - return req3; - } - - - cav_msgs::MobilityResponse getResponse1(){ - return res1; - } - - cav_msgs::MobilityResponse getResponse2(){ - return res2; - } - - cav_msgs::MobilityResponse getResponse3(){ - return res3; - } - - cav_msgs::MobilityOperation getOperation1(){ - return op1; - } - - cav_msgs::MobilityOperation getOperation2(){ - return op2; - } - - cav_msgs::MobilityOperation getOperation3(){ - return op3; - } - -}; diff --git a/platooning_strategic/test/test_platoon_manager.cpp b/platooning_strategic/test/test_platoon_manager.cpp deleted file mode 100644 index f5d920fac4..0000000000 --- a/platooning_strategic/test/test_platoon_manager.cpp +++ /dev/null @@ -1,616 +0,0 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-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 "platoon_manager.h" -#include "platoon_strategic.h" -#include "platoon_config.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace platoon_strategic -{ - TEST(PlatoonManagerTest, test_construct) - { - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin.pm_.current_platoon_state = PlatoonState::LEADER; - - } - - TEST(PlatoonManagerTest, test_ecef_encode) - { - ros::Time::init(); - - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - cav_msgs::LocationECEF ecef_point_test; - ecef_point_test.ecef_x = 1.0; - ecef_point_test.ecef_y = 2.0; - ecef_point_test.ecef_z = 3.0; - plugin.pose_ecef_point_ = ecef_point_test; - plugin.run_leader_waiting(); - - } - - - TEST(PlatoonManagerTest, test_split) - { - cav_msgs::MobilityOperation msg; - std::string strategyParams("INFO|REAR:1,LENGTH:2,SPEED:3,SIZE:4"); - std::vector inputsParams; - boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); - std::vector rearVehicleBsmId_parsed; - boost::algorithm::split(rearVehicleBsmId_parsed, inputsParams[0], boost::is_any_of(":")); - std::string rearVehicleBsmId = rearVehicleBsmId_parsed[1]; - std::cout << "rearVehicleBsmId: " << rearVehicleBsmId << std::endl; - - std::vector rearVehicleDtd_parsed; - boost::algorithm::split(rearVehicleDtd_parsed, inputsParams[1], boost::is_any_of(":")); - double rearVehicleDtd = std::stod(rearVehicleDtd_parsed[1]); - std::cout << "rearVehicleDtd: " << rearVehicleDtd << std::endl; - - // Test JOIN_REQUIREMENTS message - std::string strategyParams2("JOIN_REQUIREMENTS|LANE_INDEX:3"); - std::vectorinputsParams2; - boost::algorithm::split(inputsParams2, strategyParams2, boost::is_any_of(",")); - std::vector lane_index_parsed; - boost::algorithm::split(lane_index_parsed, inputsParams2[0], boost::is_any_of(":")); - int lane_index_integer = std::stoi(lane_index_parsed[1]); - EXPECT_EQ(lane_index_integer, 3); - } - - - // TEST(PlatoonManagerTest, test_states) - // { - // ros::Time::init(); - - // PlatoonPluginConfig config; - // std::shared_ptr wm = std::make_shared(); - - // PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - // plugin.pm_.current_platoon_state = PlatoonState::LEADER; - // plugin.pm_.current_downtrack_distance_ = 20; - - // cav_msgs::MobilityRequest request; - // request.plan_type.type = 3; - // request.strategy_params = "SIZE:1,SPEED:0,DTD:11.5599"; - - // plugin.mob_req_cb(request); - - // EXPECT_EQ(plugin.pm_.current_platoon_state, PlatoonState::LEADERWAITING); - // } - - TEST(PlatoonManagerTest, test_compose) - { - std::string OPERATION_STATUS_PARAMS = "STATUS|CMDSPEED:%1%,DTD:%2%,SPEED:%3%"; - double cmdSpeed = 1; - double current_speed = 2; - double current_downtrack = 4; - boost::format fmter(OPERATION_STATUS_PARAMS); - fmter %cmdSpeed; - fmter %current_downtrack; - fmter %current_speed; - std::string statusParams = fmter.str(); - - std::cout << "statusParams: " << statusParams << std::endl; - - // Test composed JOIN_REQUIREMENTS message - std::string JOIN_REQUIREMENTS_PARAMS = "JOIN_REQUIREMENTS|LANE_INDEX:%1%"; - int lane_index = 3; - boost::format fmter2(JOIN_REQUIREMENTS_PARAMS); - fmter2 %lane_index; - std::string joinRequirementsParams = fmter2.str(); - EXPECT_EQ(joinRequirementsParams, "JOIN_REQUIREMENTS|LANE_INDEX:3"); - } - - - TEST(PlatoonStrategicPlugin, mob_resp_cb) - { - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin.pm_.current_platoon_state = PlatoonState::FOLLOWER; - - plugin.onSpin(); - - } - - TEST(PlatoonStrategicPlugin, platoon_info_pub) - { - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin.pm_.current_platoon_state = PlatoonState::LEADER; - - cav_msgs::PlatooningInfo info_msg1 = plugin.composePlatoonInfoMsg(); - EXPECT_EQ(info_msg1.leader_id, "default_id"); - - plugin.pm_.current_platoon_state = PlatoonState::FOLLOWER; - plugin.pm_.isFollower = true; - PlatoonMember member = PlatoonMember("1", "1", 1.0, 1.1, 0.1, 100); - std::vector cur_pl; - cur_pl.push_back(member); - plugin.pm_.platoon = cur_pl; - - - cav_msgs::PlatooningInfo info_msg2 = plugin.composePlatoonInfoMsg(); - EXPECT_EQ(info_msg2.leader_id, "1"); - - } - - TEST(PlatoonStrategicPlugin, test_follower) - { - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin.pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; - plugin.pm_.current_plan.valid = true; - EXPECT_EQ(plugin.pm_.isFollower, false); - - cav_msgs::MobilityResponse resp; - resp.m_header.plan_id = "resp"; - resp.is_accepted = true; - plugin.mob_resp_cb(resp); - EXPECT_EQ(plugin.pm_.current_platoon_state, PlatoonState::FOLLOWER); - EXPECT_EQ(plugin.pm_.isFollower, true); - } - - TEST(PlatoonStrategicPlugin, test_get_leader) - { - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin.pm_.current_platoon_state = PlatoonState::FOLLOWER; - - PlatoonMember member = PlatoonMember("1", "1", 1.0, 1.1, 0.1, 100); - std::vector cur_pl; - cur_pl.push_back(member); - - plugin.pm_.platoon = cur_pl; - - EXPECT_EQ(plugin.pm_.platoon.size(), 1); - - PlatoonMember member1 = plugin.pm_.platoon[0]; - - plugin.pm_.isFollower = true; - PlatoonMember platoon_leader = plugin.pm_.getLeader(); - - EXPECT_EQ(member1.staticId, "1"); - - EXPECT_EQ(platoon_leader.staticId, "1"); - - } - - - TEST(PlatoonManagerTest, test2) - { - platoon_strategic::PlatoonMember* member = new platoon_strategic::PlatoonMember("1", "1", 1.0, 1.1, 0.1, 100); - std::vector cur_pl; - - cur_pl.push_back(*member); - - platoon_strategic::PlatoonManager pm; - pm.platoon = cur_pl; - - pm.isFollower = true; - pm.platoonSize = 1; - pm.leaderID = "0"; - pm.currentPlatoonID = "a"; - - std::string params = "CMDSPEED:11,DOWNTRACK:01,SPEED:11"; - - ros::Time::init(); - - pm.updatesOrAddMemberInfo("2", "2", 2.0, 1.0, 2.5); - - EXPECT_EQ(2, pm.platoon.size()); - EXPECT_EQ("1", pm.platoon[0].staticId); - - } - - - TEST(PlatoonManagerTest, test3) - { - platoon_strategic::PlatoonMember* member1 = new platoon_strategic::PlatoonMember("1", "1", 1.0, 1.1, 0.1, 100); - platoon_strategic::PlatoonMember* member2 = new platoon_strategic::PlatoonMember("2", "2", 2.0, 2.1, 0.2, 200); - std::vector cur_pl; - - cur_pl.push_back(*member1); - cur_pl.push_back(*member2); - - platoon_strategic::PlatoonManager pm; - pm.platoon = cur_pl; - - pm.isFollower = false; - pm.platoonSize = 2; - pm.leaderID = "0"; - pm.currentPlatoonID = "a"; - - ros::Time::init(); - - int res = pm.getTotalPlatooningSize(); - - EXPECT_EQ(3, res); - - } - - TEST(PlatoonManagerTest, test4) - { - platoon_strategic::PlatoonMember* member1 = new platoon_strategic::PlatoonMember("1", "1", 1.0, 1.1, 0.1, 100); - platoon_strategic::PlatoonMember* member2 = new platoon_strategic::PlatoonMember("2", "2", 2.0, 2.1, 0.2, 200); - std::vector cur_pl; - - cur_pl.push_back(*member1); - cur_pl.push_back(*member2); - - platoon_strategic::PlatoonManager pm; - pm.platoon = cur_pl; - - pm.isFollower = true; - pm.platoonSize = 2; - pm.leaderID = "0"; - pm.currentPlatoonID = "a"; - - - ros::Time::init(); - - int res = pm.allPredecessorFollowing(); - - EXPECT_EQ(0, res); - - } - - TEST(PlatoonManagerTest, test5) - { - // File to process. - std::string file = "/workspaces/carma_ws/src/carma-platform/platooning_strategic/test/town01_vector_map_lane_change.osm"; - lanelet::Id start_id = 101; - lanelet::Id end_id = 111; - int projector_type = 0; - std::string target_frame; - lanelet::ErrorMessages load_errors; - // Parse geo reference info from the original lanelet map (.osm) - lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame); - lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); - lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors); - if (map->laneletLayer.size() == 0) - { - FAIL() << "Input map does not contain any lanelets"; - } - std::shared_ptr cmw=std::make_shared(); - cmw->carma_wm::CARMAWorldModel::setMap(map); - //Set Route - carma_wm::test::setRouteByIds({start_id,end_id},cmw); - cmw->carma_wm::CARMAWorldModel::setMap(map); - - //get starting position - auto llt=map.get()->laneletLayer.get(start_id); - lanelet::BasicPoint2d curr_pose = llt.centerline2d().front(); - - PlatoonPluginConfig config; - config.maxCrosstrackError = 110; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(cmw, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin.pm_.current_platoon_state = PlatoonState::LEADER; - - - std::string base_proj = lanelet::projection::LocalFrameProjector::ECEF_PROJ_STR; - std_msgs::String proj_msg; - proj_msg.data = base_proj; - std_msgs::StringConstPtr msg_ptr(new std_msgs::String(proj_msg)); - plugin.georeference_cb(msg_ptr); // Set projection - - geometry_msgs::PoseStamped pose_msg; - //Assign vehicle position - pose_msg.pose.position.x = curr_pose.x(); - pose_msg.pose.position.y = curr_pose.y(); - auto mpt = boost::make_shared(pose_msg); - plugin.pose_cb(mpt); - - plugin.pm_.current_downtrack_distance_ = 150; - - cav_msgs::MobilityRequest request; - request.plan_type.type = 3; - request.strategy_params = "SIZE:1,SPEED:0,DTD:11.5599,ECEFX:1.0,ECEFY:200.0,ECEFZ:3.0"; - - plugin.single_lane_road_ = false; - plugin.in_rightmost_lane_ = true; - - plugin.mob_req_cb(request); - - EXPECT_EQ(plugin.pm_.current_platoon_state, PlatoonState::LEADER); - } - - TEST(PlatoonManagerTest, test6) - { - // File to process. - std::string file = "/workspaces/carma_ws/src/carma-platform/platooning_strategic/test/town01_vector_map_lane_change.osm"; - lanelet::Id start_id = 101; - lanelet::Id end_id = 111; - int projector_type = 0; - std::string target_frame; - lanelet::ErrorMessages load_errors; - // Parse geo reference info from the original lanelet map (.osm) - lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame); - lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); - lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors); - if (map->laneletLayer.size() == 0) - { - FAIL() << "Input map does not contain any lanelets"; - } - std::shared_ptr cmw=std::make_shared(); - cmw->carma_wm::CARMAWorldModel::setMap(map); - //Set Route - carma_wm::test::setRouteByIds({start_id,end_id},cmw); - cmw->carma_wm::CARMAWorldModel::setMap(map); - - //get starting position - auto llt=map.get()->laneletLayer.get(start_id); - lanelet::BasicPoint2d curr_pose = llt.centerline2d().front(); - - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - - PlatoonStrategicPlugin plugin(cmw, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin.pm_.current_platoon_state = PlatoonState::LEADER; - std::string base_proj = lanelet::projection::LocalFrameProjector::ECEF_PROJ_STR; - std_msgs::String proj_msg; - proj_msg.data = base_proj; - std_msgs::StringConstPtr msg_ptr(new std_msgs::String(proj_msg)); - plugin.georeference_cb(msg_ptr); // Set projection - - - geometry_msgs::PoseStamped pose_msg; - //Assign vehicle position - pose_msg.pose.position.x = curr_pose.x(); - pose_msg.pose.position.y = curr_pose.y(); - auto mpt = boost::make_shared(pose_msg); - plugin.pose_cb(mpt); - - EXPECT_EQ(plugin.single_lane_road_, false); - EXPECT_EQ(plugin.in_rightmost_lane_, false); - - } - - TEST(PlatoonStrategicPlugin, test_platoon_formation_lane_conditions) - { - // Use Guidance Lib to create map - carma_wm::test::MapOptions options; - options.lane_length_ = 20; - options.lane_width_ = 3.7; - std::shared_ptr cmw = std::make_shared(); - // Create the Semantic Map - lanelet::LaneletMapPtr map = carma_wm::test::buildGuidanceTestMap(options.lane_width_, options.lane_length_); - // Set carma_wm with this map along with its speed limit and a route - cmw->carma_wm::CARMAWorldModel::setMap(map); - carma_wm::test::setSpeedLimit(15_mph, cmw); - carma_wm::test::setRouteByIds({1210, 1213}, cmw); - // Create a PlatoonStrategicPlugin for both a front and a rear vehicle and set their initial platoon state to Leader - PlatoonPluginConfig config; - PlatoonStrategicPlugin plugin_front(cmw, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin_front.platooning_enabled_ = true; - plugin_front.pm_.current_platoon_state = PlatoonState::LEADER; - PlatoonStrategicPlugin plugin_rear(cmw, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin_rear.platooning_enabled_ = true; - plugin_rear.pm_.current_platoon_state = PlatoonState::LEADER; - // Set georeference projection for front and rear vehicle - std::string base_proj = "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs"; - std_msgs::String georeference_msg; - georeference_msg.data = base_proj; - std_msgs::StringConstPtr msg_ptr(new std_msgs::String(georeference_msg)); - plugin_front.georeference_cb(msg_ptr); - plugin_rear.georeference_cb(msg_ptr); - //////// TEST 1: Front vehicle transitions LEADER -> LEADERWAITING after receiving a valid ///////////// - //////// JOIN_PLATOON_AT_REAR request if Front vehicle is in a suitable platooning lane ///////////// - - // Place front vehicle in 1211 (middle lane second lanelet) - geometry_msgs::PoseStamped msg; - msg.pose.position.x = 4; - msg.pose.position.y = 25; - geometry_msgs::PoseStampedPtr mpt2(new geometry_msgs::PoseStamped(msg)); - plugin_front.pose_cb(mpt2); - EXPECT_EQ(plugin_front.in_rightmost_lane_, false); - plugin_front.pm_.current_downtrack_distance_ = 20; - // Place rear vehicle in 1210 (middle lane first lanelet) and obtain the ECEF location of this point - msg.pose.position.x = 4; - msg.pose.position.y = 10; - geometry_msgs::PoseStampedPtr mpt(new geometry_msgs::PoseStamped(msg)); - plugin_rear.pose_cb(mpt); - cav_msgs::LocationECEF rear_vehicle_location = plugin_rear.pose_ecef_point_; - - // Create the rear vehicle's 'JOIN_PLATOON_AT_REAR' MobilityRequest, which will be sent to the front vehicle - cav_msgs::MobilityRequest request; - request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); - request.m_header.recipient_id = "default_recipient_id"; - request.m_header.sender_bsm_id = "default_host_id"; - request.m_header.sender_id = "default_id"; - request.m_header.timestamp = ros::Time::now().toNSec()/1000000; - request.location = rear_vehicle_location; // Rear vehicle is in Lanelet 1210 - request.plan_type.type = cav_msgs::PlanType::JOIN_PLATOON_AT_REAR; - request.strategy = "Carma/Platooning"; - request.strategy_params = "SIZE:0,SPEED:4.0,DTD:15.0,ECEFX:4.0,ECEFY:10.0,ECEFZ:0.0"; - request.urgency = 50; - // Front vehicle receives the 'JOIN_PLATOON_AT_REAR' MobilityRequest and transitions to LEADERWAITING - plugin_front.handle_mob_req(request); - EXPECT_EQ(plugin_front.pm_.current_platoon_state, PlatoonState::LEADERWAITING); - EXPECT_EQ(plugin_rear.pm_.current_platoon_state, PlatoonState::LEADER); - - //////// TEST 2: Front vehicle remains LEADER after receiving a valid JOIN_PLATOON_AT_REAR ///////////// - //////// request if the front vehicle is in a rightmost lane. Front vehicle transitions ///////////// - //////// LEADER -> LEADERWAITING only after entering a non-rightmost lane. ///////////// - - // Change front vehicle back to LEADER state and place it in lanelet 1221 (rightmost lane) - plugin_front.pm_.current_platoon_state = PlatoonState::LEADER; - msg.pose.position.x = 10; - msg.pose.position.y = 25; - geometry_msgs::PoseStampedPtr mpt3(new geometry_msgs::PoseStamped(msg)); - plugin_front.pose_cb(mpt3); - EXPECT_EQ(plugin_front.in_rightmost_lane_, true); - // Place rear vehicle in lanelet 1220 (also rightmost lane) - msg.pose.position.x = 10; - msg.pose.position.y = 9; - geometry_msgs::PoseStampedPtr mpt4(new geometry_msgs::PoseStamped(msg)); - plugin_rear.pose_cb(mpt4); - rear_vehicle_location = plugin_rear.pose_ecef_point_; - EXPECT_EQ(plugin_front.in_rightmost_lane_, true); - // Create the rear vehicle's 'JOIN_PLATOON_AT_REAR' MobilityRequest, which will be sent to the front vehicle - request.location = rear_vehicle_location; // Rear vehicle is in Lanelet 1220 - request.strategy_params = "SIZE:0,SPEED:4.0,DTD:15.0,ECEFX:10.0,ECEFY:9.0,ECEFZ:0.0"; - // Front vehicle receives the 'JOIN_PLATOON_AT_REAR' MobilityRequest and does not transition to LEADERWAITING since it is in rightmost lane - EXPECT_EQ(plugin_front.leader_lane_change_required_, false); - plugin_front.handle_mob_req(request); - EXPECT_EQ(plugin_front.pm_.current_platoon_state, PlatoonState::LEADER); - EXPECT_EQ(plugin_front.current_lane_index_, 0); - EXPECT_EQ(plugin_front.current_lane_group_size_, 3); - EXPECT_EQ(plugin_front.leader_lane_change_required_, true); - EXPECT_EQ(plugin_rear.pm_.current_platoon_state, PlatoonState::LEADER); - // Place front vehicle in lanelet 1211 so it is now in the middle lane (a suitable platooning lane) - msg.pose.position.x = 4; - msg.pose.position.y = 25; - geometry_msgs::PoseStampedPtr mpt5(new geometry_msgs::PoseStamped(msg)); - plugin_front.pose_cb(mpt5); - plugin_front.onSpin(); // Trigger state transition - EXPECT_EQ(plugin_front.in_rightmost_lane_, false); - EXPECT_EQ(plugin_front.current_lane_index_, 1); - EXPECT_EQ(plugin_front.leader_lane_change_required_, false); - EXPECT_EQ(plugin_front.pm_.current_platoon_state, PlatoonState::LEADERWAITING); - // Rear vehicle receives ACK - plugin_rear.pm_.current_plan.valid = true; - cav_msgs::MobilityResponse response; - response.is_accepted = true; - plugin_rear.mob_resp_cb(response); - EXPECT_EQ(plugin_rear.pm_.current_platoon_state, PlatoonState::CANDIDATEFOLLOWER); - // Create JOIN_REQUIREMENTS MobilityOperation for front vehicle to send to rear vehicle - plugin_front.config_.vehicleID = "Front-ID"; - cav_msgs::MobilityOperation join_requirements = plugin_front.composeMobilityOperationLeaderWaiting("JOIN_REQUIREMENTS"); - EXPECT_EQ(join_requirements.strategy_params, "JOIN_REQUIREMENTS|LANE_INDEX:1,LANE_GROUP_SIZE:3"); - // Rear vehicle receives JOIN_REQUIREMENTS MobilityOperation, which includes the target_lane_index - plugin_rear.pm_.targetLeaderId = "Front-ID"; - EXPECT_EQ(plugin_rear.has_received_join_requirements_, false); - plugin_rear.mob_op_cb(join_requirements); - EXPECT_EQ(plugin_rear.has_received_join_requirements_, true); - EXPECT_EQ(plugin_rear.cf_target_lane_index_, 1); - EXPECT_EQ(plugin_rear.cf_lane_change_required_, true); - // Rear vehicle enters target_lane_index and is no longer required to change lanes - msg.pose.position.x = 4; - msg.pose.position.y = 10; - geometry_msgs::PoseStampedPtr mpt6(new geometry_msgs::PoseStamped(msg)); - plugin_rear.pose_cb(mpt6); - plugin_rear.onSpin(); // Trigger state transition - EXPECT_EQ(plugin_rear.cf_lane_change_required_, false); - } - - TEST(PlatoonStrategicPlugin, test_platoon_lane_change) - { - // Use Guidance Lib to create map - carma_wm::test::MapOptions options; - options.lane_length_ = 20; - options.lane_width_ = 3.7; - std::shared_ptr cmw = std::make_shared(); - // Create the Semantic Map - lanelet::LaneletMapPtr map = carma_wm::test::buildGuidanceTestMap(options.lane_width_, options.lane_length_); - - // Set carma_wm with this map along with its speed limit and a route - cmw->carma_wm::CARMAWorldModel::setMap(map); - carma_wm::test::setSpeedLimit(15_mph, cmw); - carma_wm::test::setRouteByIds({1220, 1221}, cmw); - - // Create a PlatoonStrategicPlugin for both a front and a rear vehicle and set their initial platoon state to Leader - PlatoonPluginConfig config; - PlatoonStrategicPlugin plugin1(cmw, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - plugin1.platooning_enabled_ = true; - plugin1.pm_.current_platoon_state = PlatoonState::LEADER; - - // Add member to the platoon - platoon_strategic::PlatoonMember member = platoon_strategic::PlatoonMember("1", "1", 1.0, 1.1, 0.1, 100); - std::vector cur_pl; - cur_pl.push_back(member); - plugin1.pm_.platoon = cur_pl; - - // Set georeference projection for front and rear vehicle - std::string base_proj = "+proj=tmerc +lat_0=0 +lon_0=0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +geoidgrids=egm96_15.gtx +vunits=m +no_defs"; - std_msgs::String georeference_msg; - georeference_msg.data = base_proj; - std_msgs::StringConstPtr msg_ptr(new std_msgs::String(georeference_msg)); - plugin1.georeference_cb(msg_ptr); - - geometry_msgs::PoseStamped msg; - msg.pose.position.x = 10; - msg.pose.position.y = 8; - geometry_msgs::PoseStampedPtr mpt3(new geometry_msgs::PoseStamped(msg)); - plugin1.pose_cb(mpt3); - plugin1.leader_lane_change_required_ = true; - - cav_srvs::PlanManeuversRequest req; - req.prior_plan.maneuvers = {}; - cav_srvs::PlanManeuversResponse resp; - ros::Time::init(); - - if (plugin1.plan_maneuver_cb(req, resp)) - { - ASSERT_EQ(cav_msgs::Maneuver::LANE_CHANGE, resp.new_plan.maneuvers.front().type); - } - - plugin1.pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; - plugin1.cf_target_lane_index_ = 1; - plugin1.cf_lane_change_required_ = true; - - cav_srvs::PlanManeuversRequest req1; - req.prior_plan.maneuvers = {}; - cav_srvs::PlanManeuversResponse resp1; - if (plugin1.plan_maneuver_cb(req1, resp1)) - { - ASSERT_EQ(cav_msgs::Maneuver::LANE_CHANGE, resp1.new_plan.maneuvers.front().type); - } - - - - } -} - - diff --git a/platooning_strategic/test/town01_vector_map_lane_change.osm b/platooning_strategic/test/town01_vector_map_lane_change.osm deleted file mode 100755 index 85a1b7ed45..0000000000 --- a/platooning_strategic/test/town01_vector_map_lane_change.osm +++ /dev/null @@ -1,55348 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/platooning_strategic_IHP/CMakeLists.txt b/platooning_strategic_IHP/CMakeLists.txt index 253a3a10a9..a750e25230 100644 --- a/platooning_strategic_IHP/CMakeLists.txt +++ b/platooning_strategic_IHP/CMakeLists.txt @@ -1,119 +1,85 @@ -# 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. - -cmake_minimum_required(VERSION 2.8.3) -project(platoon_strategic_ihp) - -find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) -carma_package() - -## Compile as C++11, supported in ROS Kinetic and newer -#add_compile_options(-std=c++14) -#set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") -#set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - carma_utils - cav_msgs - cav_srvs - roscpp - std_msgs - carma_wm - lanelet2_extension - tf - tf2 - tf2_ros - tf2_geometry_msgs -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED) - -################################### -## catkin specific configuration ## -################################### - - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS carma_utils cav_msgs roscpp std_msgs carma_wm cav_srvs lanelet2_extension -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - -#file(GLOB_RECURSE headers */*.hpp */*.h) - -add_library(platoon_strategic_ihp_plugin_lib - src/platoon_strategic_ihp.cpp - src/platoon_manager_ihp.cpp) - -add_dependencies(platoon_strategic_ihp_plugin_lib ${catkin_EXPORTED_TARGETS}) -target_link_libraries(platoon_strategic_ihp_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -add_executable( ${PROJECT_NAME} - src/main_ihp.cpp) - -target_link_libraries(${PROJECT_NAME} platoon_strategic_ihp_plugin_lib) - - -############# -## Install ## -############# - - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE -) - -## Install C++ -install(TARGETS ${PROJECT_NAME} platoon_strategic_ihp_plugin_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - - -############# -## Testing ## -############# - -# catkin_add_gmock(${PROJECT_NAME}-test -# test/test_platoon_manager_front_join.cpp -# test/test_platoon_manager.cpp -# test/mobility_messages.cpp -# test/main_test.cpp) - -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test platoon_strategic_ihp_plugin_lib ${catkin_LIBRARIES}) -# endif() + +# 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. + +cmake_minimum_required(VERSION 3.5) +project(platoon_strategic_ihp) + +# Declare carma package and check ROS version +find_package(carma_cmake_common REQUIRED) +carma_check_ros_version(2) +carma_package() + +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Name build targets +set(node_exec platoon_strategic_ihp_node_exec) +set(node_lib platoon_strategic_ihp_node) + +# Includes +include_directories( + include +) + +# Build +ament_auto_add_library(${node_lib} SHARED + src/platoon_manager_ihp.cpp + src/platoon_strategic_ihp_node.cpp + src/platoon_strategic_ihp.cpp +) + +ament_auto_add_executable(${node_exec} + src/main.cpp +) + +# Register component +rclcpp_components_register_nodes(${node_lib} "platoon_strategic_ihp::Node") + +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(test_platoon_strategic_ihp + test/main_test.cpp + test/mobility_messages.cpp + test/test_platoon_manager_front_join.cpp + test/test_platoon_manager.cpp + ) + + ament_target_dependencies(test_platoon_strategic_ihp ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_platoon_strategic_ihp ${node_lib}) + +endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/platooning_strategic_IHP/README.md b/platooning_strategic_IHP/README.md new file mode 100644 index 0000000000..ce67fb0c8e --- /dev/null +++ b/platooning_strategic_IHP/README.md @@ -0,0 +1,9 @@ +# platoon_strategic_ihp + +Strategic plugin for the Integrated Highway Prototype 2 (IHP2) implementation which allows platooning to include front and rear merge joins. Note despite the name, as of Aug 2022 testing for this IHP2 implementation was only up to 35mph on a closed test track per limitations of the CARMA Platform system. + +Overview +https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2138079233/Integrated+Highway+Prototype+2+IHP2 + +Design +https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2138767361/IHP2+Platooning+Plugin diff --git a/platooning_strategic_IHP/include/platoon_config_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_config_ihp.h similarity index 98% rename from platooning_strategic_IHP/include/platoon_config_ihp.h rename to platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_config_ihp.h index ff48bccf16..7bbe4e10a2 100644 --- a/platooning_strategic_IHP/include/platoon_config_ihp.h +++ b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_config_ihp.h @@ -18,6 +18,9 @@ #include +namespace platoon_strategic_ihp +{ + /** * \brief Stuct containing the algorithm configuration values for the yield_pluginConfig */ @@ -115,7 +118,9 @@ struct PlatoonPluginConfig << "allowCutinJoin: " << c.allowCutinJoin << std::endl << "significantDTDchange: " << c.significantDTDchange << std::endl << "join_index: " << c.join_index << std::endl - << "}" << std::endl; + << "}" << std::endl; //TODO this is missing some fields return output; } }; + +} //platoon_strategic_ihp diff --git a/platooning_strategic_IHP/include/platoon_manager_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_manager_ihp.h similarity index 96% rename from platooning_strategic_IHP/include/platoon_manager_ihp.h rename to platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_manager_ihp.h index cacb908892..b48893c8f7 100644 --- a/platooning_strategic_IHP/include/platoon_manager_ihp.h +++ b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_manager_ihp.h @@ -23,18 +23,18 @@ * Author: Xu Han, Xin Xia, Jiaqi Ma */ -#include -#include -#include -#include -#include -#include -#include -#include -#include + +#include +#include +#include +#include +#include +#include +#include +#include #include #include -#include +#include #include "platoon_config_ihp.h" namespace platoon_strategic_ihp @@ -120,9 +120,11 @@ namespace platoon_strategic_ihp public: /** - * \brief Default constructor + * \brief Constructor + * + * \param timer_factory An interface which can be used to get access to the current time */ - PlatoonManager(); + PlatoonManager(std::shared_ptr timer_factory); /** * \brief Stores the latest info on location of the host vehicle. @@ -394,7 +396,7 @@ namespace platoon_strategic_ihp bool is_neighbor_record_complete_ = false; // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; + geometry_msgs::msg::PoseStamped pose_msg_; // host vehicle's static ID std::string HostMobilityId = "default_host_id"; @@ -403,6 +405,9 @@ namespace platoon_strategic_ihp bool isCreateGap = false; size_t dynamic_leader_index_ = 0; + + // Timer factory used to get current time + std::shared_ptr timer_factory_; private: diff --git a/platooning_strategic_IHP/include/platoon_strategic_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_ihp.h similarity index 79% rename from platooning_strategic_IHP/include/platoon_strategic_ihp.h rename to platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_ihp.h index da4f45329f..475af88ae7 100644 --- a/platooning_strategic_IHP/include/platoon_strategic_ihp.h +++ b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_ihp.h @@ -23,90 +23,81 @@ #pragma once +#include #include -#include #include -#include -#include -#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include +#include #include "platoon_config_ihp.h" -#include +#include "platoon_manager_ihp.h" #include #include #include #include -#include -#include +#include +#include namespace platoon_strategic_ihp { - using PublishPluginDiscoveryCB = std::function; - using MobilityResponseCB = std::function; - using MobilityRequestCB = std::function; - using MobilityOperationCB = std::function; - using PlatooningInfoCB = std::function; + using MobilityResponseCB = std::function; + using MobilityRequestCB = std::function; + using MobilityOperationCB = std::function; + using PlatooningInfoCB = std::function; class PlatoonStrategicIHPPlugin { public: - - /** - * \brief Default constructor for PlatoonStrategicIHPPlugin class - */ - PlatoonStrategicIHPPlugin(); /** * \brief Constructor * - * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data + * \param wm Pointer to initalized instance of the carma world model for accessing semantic map data * \param config The configuration to be used for this object - * \param plugin_discovery_publisher Callback which will publish the current plugin discovery state */ - PlatoonStrategicIHPPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher, MobilityResponseCB mobility_response_publisher, + PlatoonStrategicIHPPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, MobilityResponseCB mobility_response_publisher, MobilityRequestCB mobility_request_publisher, MobilityOperationCB mobility_operation_publisher, - PlatooningInfoCB platooning_info_publisher); + PlatooningInfoCB platooning_info_publisher, + std::shared_ptr timer_factory); /** * \brief Callback function for Mobility Operation Message * * \param msg Mobility Operation Message */ - void mob_op_cb(const cav_msgs::MobilityOperation& msg); + void mob_op_cb(const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg); /** * \brief Callback function for Mobility Request Message * * \param msg Mobility Request Message */ - void mob_req_cb(const cav_msgs::MobilityRequest& msg); + void mob_req_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg); /** * \brief Callback function for Mobility Response Message * * \param msg Mobility Response Message */ - void mob_resp_cb(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg); /** * \brief Function to the process and respond to the mobility request @@ -115,7 +106,7 @@ namespace platoon_strategic_ihp * * \return Mobility response message */ - MobilityRequestResponse handle_mob_req(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse handle_mob_req(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Callback function to the maneuver request @@ -125,7 +116,7 @@ namespace platoon_strategic_ihp * * \return Mobility response message */ - bool plan_maneuver_cb(cav_srvs::PlanManeuversRequest &req, cav_srvs::PlanManeuversResponse &resp); + bool plan_maneuver_cb(carma_planning_msgs::srv::PlanManeuvers::Request &req, carma_planning_msgs::srv::PlanManeuvers::Response &resp); /** * \brief Find lanelet index from path @@ -156,7 +147,7 @@ namespace platoon_strategic_ihp * * \return Maneuver message */ - cav_msgs::Maneuver composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time& current_time); + carma_planning_msgs::msg::Maneuver composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, rclcpp::Time& current_time); /** * \brief Find start(current) and target(end) lanelet index from path to generate lane change maneuver message. @@ -171,7 +162,7 @@ namespace platoon_strategic_ihp * * \return Maneuver message */ - cav_msgs::Maneuver composeLaneChangeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int starting_lane_id, int ending_lane_id, ros::Time& current_time); + carma_planning_msgs::msg::Maneuver composeLaneChangeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int starting_lane_id, int ending_lane_id, rclcpp::Time& current_time); /** * \brief Update maneuver status based on prior plan @@ -181,14 +172,14 @@ namespace platoon_strategic_ihp * \param current_progress current progress * \param lane_id lanelet ud */ - void updateCurrentStatus(cav_msgs::Maneuver maneuver, double& speed, double& current_progress, int& lane_id); + void updateCurrentStatus(carma_planning_msgs::msg::Maneuver maneuver, double& speed, double& current_progress, int& lane_id); /** * \brief Callback function for current pose * * \param msg PoseStamped msg */ - void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); + void pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg); /** * \brief Compose Platoon information message @@ -214,25 +205,25 @@ namespace platoon_strategic_ihp * to the leader. * */ - cav_msgs::PlatooningInfo composePlatoonInfoMsg(); + carma_planning_msgs::msg::PlatooningInfo composePlatoonInfoMsg(); /** * \brief Callback for the twist subscriber, which will store latest twist locally * \param msg Latest twist message */ - void twist_cb(const geometry_msgs::TwistStampedConstPtr& msg); + void twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg); /** * \brief Callback for the control command * \param msg Latest twist cmd message */ - void cmd_cb(const geometry_msgs::TwistStampedConstPtr& msg); + void cmd_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg); /** * \brief Callback for the georeference * \param msg Latest georeference */ - void georeference_cb(const std_msgs::StringConstPtr& msg); + void georeference_cb(const std_msgs::msg::String::UniquePtr msg); /** * \brief Spin callback function @@ -260,7 +251,7 @@ namespace platoon_strategic_ihp void run_follower(); // ECEF position of the host vehicle - cav_msgs::LocationECEF pose_ecef_point_; + carma_v2x_msgs::msg::LocationECEF pose_ecef_point_; // -------------- UCLA: add two states for frontal join ------------------ /** @@ -287,7 +278,7 @@ namespace platoon_strategic_ihp /** * \brief UCLA Update the private variable pose_ecef_point_ */ - void setHostECEF(cav_msgs::LocationECEF pose_ecef_point); + void setHostECEF(carma_v2x_msgs::msg::LocationECEF pose_ecef_point); /** * \brief UCLA Getter: for PlatoonManager class @@ -303,26 +294,21 @@ namespace platoon_strategic_ihp * \brief UCLA Setter: Update platoon list (Unit Test). */ void updatePlatoonList(std::vector platoon_list); - + /** - * \brief UCLA Setter: Set the host to follower state (Unit Test). - */ - void setToFollower(); + * \brief Set the current config + */ + void setConfig(const PlatoonPluginConfig& config); private: - PublishPluginDiscoveryCB plugin_discovery_publisher_; MobilityRequestCB mobility_request_publisher_; MobilityResponseCB mobility_response_publisher_; MobilityOperationCB mobility_operation_publisher_; PlatooningInfoCB platooning_info_publisher_; - // Platoon Manager Object - PlatoonManager pm_; - - // wm listener pointer and pointer to the actual wm object - std::shared_ptr wml_; + // pointer to the actual wm object carma_wm::WorldModelConstPtr wm_; // local copy of configuration file @@ -330,7 +316,7 @@ namespace platoon_strategic_ihp // local copy of pose // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; + geometry_msgs::msg::PoseStamped pose_msg_; //Internal Variables used in unit tests // Current vehicle command speed, m/s @@ -349,17 +335,6 @@ namespace platoon_strategic_ihp // Host Mobility ID std::string HostMobilityId = "hostid"; - // ROS Publishers - ros::Publisher platoon_strategic_ihp_plugin_discovery_pub_; - ros::Publisher mob_op_pub_; - ros::Publisher mob_req_pub_; - - // ROS Subscribers - ros::Subscriber pose_sub_; - ros::Subscriber mob_req_sub_; - ros::Subscriber mob_resp_sub_; - ros::Subscriber mob_op_sub_; - /** * \brief Function to determine if a target vehicle is in the front of host vehicle @@ -388,7 +363,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response */ - MobilityRequestResponse mob_req_cb_leader(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_leader(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility request in leader waiting state @@ -397,7 +372,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response */ - MobilityRequestResponse mob_req_cb_leaderwaiting(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility request in follower state @@ -406,7 +381,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response */ - MobilityRequestResponse mob_req_cb_follower(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_follower(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility request in candidate follower state @@ -415,7 +390,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response */ - MobilityRequestResponse mob_req_cb_candidatefollower(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility request in standby state @@ -424,77 +399,77 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response */ - MobilityRequestResponse mob_req_cb_standby(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_standby(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility response in leader state * * \param msg incoming mobility response */ - void mob_resp_cb_leader(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_leader(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility response in leader waiting state * * \param msg incoming mobility response */ - void mob_resp_cb_leaderwaiting(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility response in follower state * * \param msg incoming mobility response */ - void mob_resp_cb_follower(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_follower(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility response in candidate follower state * * \param msg incoming mobility response */ - void mob_resp_cb_candidatefollower(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility response in standby state * * \param msg incoming mobility response */ - void mob_resp_cb_standby(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_standby(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility operation in leader state * * \param msg incoming mobility operation */ - void mob_op_cb_leader(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_leader(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation in leader waiting state * * \param msg incoming mobility operation */ - void mob_op_cb_leaderwaiting(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation in follower state * * \param msg incoming mobility operation */ - void mob_op_cb_follower(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_follower(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation in candidate follower state * * \param msg incoming mobility operation */ - void mob_op_cb_candidatefollower(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation in standby state * * \param msg incoming mobility operation */ - void mob_op_cb_standby(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_standby(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to compose mobility operation in leader state @@ -503,28 +478,28 @@ namespace platoon_strategic_ihp * * \return mobility operation msg */ - cav_msgs::MobilityOperation composeMobilityOperationLeader(const std::string& type); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeader(const std::string& type); /** * \brief Function to compose mobility operation in follower state * * \return mobility operation msg */ - cav_msgs::MobilityOperation composeMobilityOperationFollower(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationFollower(); /** * \brief Function to compose mobility operation in leader waiting state * * \return mobility operation msg */ - cav_msgs::MobilityOperation composeMobilityOperationLeaderWaiting(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeaderWaiting(); /** * \brief Function to compose mobility operation in candidate follower state * * \return mobility operation msg */ - cav_msgs::MobilityOperation composeMobilityOperationCandidateFollower(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationCandidateFollower(); /** * \brief Function to convert pose from map frame to ecef location @@ -533,7 +508,7 @@ namespace platoon_strategic_ihp * * \return mobility operation msg */ - cav_msgs::LocationECEF pose_to_ecef(geometry_msgs::PoseStamped pose_msg); + carma_v2x_msgs::msg::LocationECEF pose_to_ecef(geometry_msgs::msg::PoseStamped pose_msg); /** * \brief Function to convert ecef location to a 2d point in map frame @@ -542,7 +517,7 @@ namespace platoon_strategic_ihp * * \return 2d point in map frame */ - lanelet::BasicPoint2d ecef_to_map_point(cav_msgs::LocationECEF ecef_point); + lanelet::BasicPoint2d ecef_to_map_point(carma_v2x_msgs::msg::LocationECEF ecef_point); // -------------- UCLA implemented functions ----------------------- @@ -587,14 +562,14 @@ namespace platoon_strategic_ihp /** * \brief Function to find the starting and ending lanelet ID for lane change in a two-lane scenario (used for cut-in join scenarios). * - * Note: This is a temporary function for internal test only. The scenario is not genrealized. Can only find adjacebt lanletID based on predefiend direction (left, right). + * Note: This is a temporary function for internal test only. The scenario is not generalized. Can only find adjacent lanletID based on predefined direction (left, right). * * \TODO: This function should be replaced by the complete arbitrary lane change module. * * \param start_downtrack: The downtrack distance (m) of the starting point. * end_downtrack: The downtrack distance (m) of the target (end) point. * - * \return (int): The adjacent laneletID based on the provided dontrack distance range. + * \return (int): The adjacent laneletID based on the provided downtrack distance range. */ int find_target_lanelet_id(double start_downtrack, double end_downtrack); @@ -605,28 +580,28 @@ namespace platoon_strategic_ihp * * \return mobility operation msg. */ - cav_msgs::MobilityOperation composeMobilityOperationSTATUS(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationSTATUS(); /** * \brief Function to compose mobility operation message with INFO params. * * \return mobility operation msg. */ - cav_msgs::MobilityOperation composeMobilityOperationINFO(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationINFO(); /** * \brief Function to compose mobility operation in LeaderAborting state. * * \return mobility operation msg. */ - cav_msgs::MobilityOperation composeMobilityOperationLeaderAborting(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeaderAborting(); /** * \brief Function to compose mobility operation in CandidateLeader. * * \return mobility operation msg. */ - cav_msgs::MobilityOperation composeMobilityOperationCandidateLeader(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationCandidateLeader(); /** * \brief Function to compose mobility operation in LeadWithOperation (cut-in join) @@ -636,7 +611,7 @@ namespace platoon_strategic_ihp * \return msg: mobility operation msg * Return null ("") if the incoming message is trashed/unrecognized. */ - cav_msgs::MobilityOperation composeMobilityOperationLeadWithOperation(const std::string& type); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationLeadWithOperation(const std::string& type); /** * \brief Function to compose mobility operation in PrepareToJoin (cut-in join) @@ -646,7 +621,7 @@ namespace platoon_strategic_ihp * \return msg: mobility operation msg * Return null ("") if the incoming message is trashed/unrecognized. */ - cav_msgs::MobilityOperation composeMobilityOperationPrepareToJoin(); + carma_v2x_msgs::msg::MobilityOperation composeMobilityOperationPrepareToJoin(); // --------- 2. Mobility operation callback ----------- @@ -656,7 +631,7 @@ namespace platoon_strategic_ihp * * \param msg incoming mobility operation message. */ - void mob_op_cb_STATUS(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_STATUS(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation INFO params to find platoon length in m. @@ -674,7 +649,7 @@ namespace platoon_strategic_ihp * * \return ecef location of the sender. */ - cav_msgs::LocationECEF mob_op_find_ecef_from_INFO_params(std::string strategyParams); + carma_v2x_msgs::msg::LocationECEF mob_op_find_ecef_from_INFO_params(std::string strategyParams); /** * \brief Function to process mobility operation for STATUS params. @@ -683,35 +658,35 @@ namespace platoon_strategic_ihp * * \return ecef location of the sender. */ - cav_msgs::LocationECEF mob_op_find_ecef_from_STATUS_params(std::string strategyParams); + carma_v2x_msgs::msg::LocationECEF mob_op_find_ecef_from_STATUS_params(std::string strategyParams); /** * \brief Function to process mobility operation in leaderaborting state. * * \param msg incoming mobility operation message. */ - void mob_op_cb_leaderaborting(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation in candidateleader state. * * \param msg incoming mobility operation message. */ - void mob_op_cb_candidateleader(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_candidateleader(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation in LeadWithOperation state (cut-in join) * * \param msg incoming mobility operation */ - void mob_op_cb_leadwithoperation(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityOperation& msg); /** * \brief Function to process mobility operation in PrepareToJoin state (cut-in join) * * \param msg incoming mobility operation */ - void mob_op_cb_preparetojoin(const cav_msgs::MobilityOperation& msg); + void mob_op_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityOperation& msg); //------- 3. Mobility request callback ----------- @@ -722,7 +697,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response. */ - MobilityRequestResponse mob_req_cb_leaderaborting(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility request in candidateleader state. @@ -731,7 +706,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response. */ - MobilityRequestResponse mob_req_cb_candidateleader(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_candidateleader(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility request in leadwithoperation state (cut-in join) @@ -740,7 +715,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response */ - MobilityRequestResponse mob_req_cb_leadwithoperation(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityRequest& msg); /** * \brief Function to process mobility request in preparetojoin state (cut-in join) @@ -749,7 +724,7 @@ namespace platoon_strategic_ihp * * \return ACK, NACK, or No response */ - MobilityRequestResponse mob_req_cb_preparetojoin(const cav_msgs::MobilityRequest& msg); + MobilityRequestResponse mob_req_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityRequest& msg); // ------ 4. Mobility response callback ------ /** @@ -757,28 +732,28 @@ namespace platoon_strategic_ihp * * \param msg incoming mobility response. */ - void mob_resp_cb_leaderaborting(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility response in candidateleader state. * * \param msg incoming mobility response. */ - void mob_resp_cb_candidateleader(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_candidateleader(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility response in leadwithoperation state (cut-in join) * * \param msg incoming mobility response */ - void mob_resp_cb_leadwithoperation(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityResponse& msg); /** * \brief Function to process mobility response in preparetojoin state * * \param msg incoming mobility response */ - void mob_resp_cb_preparetojoin(const cav_msgs::MobilityResponse& msg); + void mob_resp_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityResponse& msg); // Pointer for map projector @@ -787,11 +762,8 @@ namespace platoon_strategic_ihp // flag to check if map is loaded bool map_loaded_ = false; - // ros service servers - ros::ServiceServer maneuver_srv_; - // Plugin discovery message - cav_msgs::Plugin plugin_discovery_msg_; + carma_planning_msgs::msg::Plugin plugin_discovery_msg_; // Number of calls to the run_leader_aborting() method int numLeaderAbortingCalls_ = 0; @@ -825,6 +797,12 @@ namespace platoon_strategic_ihp // Is there a sufficient gap open in the platoon for a cut-in join? bool safeToLaneChange_ = false; + // Interface for getting current time + std::shared_ptr timer_factory_; + + // Platoon Manager Object + PlatoonManager pm_; + // Strategy types const std::string PLATOONING_STRATEGY = "Carma/Platooning"; const std::string OPERATION_INFO_TYPE = "INFO"; @@ -865,5 +843,8 @@ namespace platoon_strategic_ihp * for cut-in in middle, index indicate the gap leading vehicle's index. */ const std::string JOIN_PARAMS = "SIZE:%1%,SPEED:%2%,ECEFX:%3%,ECEFY:%4%,ECEFZ:%5%,JOINIDX:%6%"; + + // Unit Test Accessors + FRIEND_TEST(PlatoonStrategicIHPPlugin, platoon_info_pub_front); }; } diff --git a/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_plugin_node_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_plugin_node_ihp.h new file mode 100644 index 0000000000..2176d24c40 --- /dev/null +++ b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_plugin_node_ihp.h @@ -0,0 +1,113 @@ +/* + * Copyright (C) 2021-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. + */ + +/* + * Originally Developed for ROS1 by the UCLA Mobility Lab, 10/20/2021. + * + * Creator: Xu Han + * Author: Xu Han, Xin Xia, Jiaqi Ma + * + * 8/15/2022: Ported to ROS2 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "platoon_strategic_ihp.h" +#include "platoon_config_ihp.h" + +namespace platoon_strategic_ihp +{ + + /** + * \brief ROS Node to for Platooning Strategic Plugin IHP2 variant + */ + class Node : public carma_guidance_plugins::StrategicPlugin + { + + private: + + // Publishers + carma_ros2_utils::PubPtr platoon_info_pub; + carma_ros2_utils::PubPtr mob_response_pub; + carma_ros2_utils::PubPtr mob_request_pub; + carma_ros2_utils::PubPtr mob_operation_pub; + + // Subscribers + carma_ros2_utils::SubPtr mob_request_sub; + carma_ros2_utils::SubPtr mob_response_sub; + carma_ros2_utils::SubPtr mob_operation_sub; + carma_ros2_utils::SubPtr current_pose_sub; + carma_ros2_utils::SubPtr current_twist_sub; + carma_ros2_utils::SubPtr cmd_sub; + carma_ros2_utils::SubPtr georeference_sub; + + // Timers + rclcpp::TimerBase::SharedPtr loop_timer_; + + // Node configuration + PlatoonPluginConfig config_; + + // Worker + std::shared_ptr worker_; + + public: + /** + * \brief Node constructor + */ + explicit Node(const rclcpp::NodeOptions &); + + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + + //// + // Overrides + //// + void plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) override; + + bool get_availability() override; + + std::string get_version_id() override; + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + carma_ros2_utils::CallbackReturn on_cleanup_plugin(); + + }; + +} // platoon_strategic_ihp diff --git a/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h deleted file mode 100644 index 3b22e07ba5..0000000000 --- a/platooning_strategic_IHP/include/platoon_strategic_plugin_node_ihp.h +++ /dev/null @@ -1,125 +0,0 @@ -#pragma once - -/* - * Copyright (C) 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. - */ - -/* - * Developed by the UCLA Mobility Lab, 10/20/2021. - * - * Creator: Xu Han - * Author: Xu Han, Xin Xia, Jiaqi Ma - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "platoon_strategic_ihp.h" -#include "platoon_config_ihp.h" - -namespace platoon_strategic_ihp -{ -/** - * \brief ROS node for the yield_plugin - */ -class PlatoonStrategicIHPPluginNode -{ -public: - - /** - * \brief Entrypoint for this node - */ - void run() - { - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - carma_wm::WMListener wml; - auto wm_ = wml.getWorldModel(); - - ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); - ros::Publisher mob_response_pub = nh.advertise("outgoing_mobility_response", 1); - ros::Publisher mob_request_pub = nh.advertise("outgoing_mobility_request", 1); - ros::Publisher mob_operation_pub = nh.advertise("outgoing_mobility_operation", 1); - ros::Publisher platoon_info_pub = nh.advertise("platoon_info", 1); - - - PlatoonPluginConfig config; - - pnh.param("maxPlatoonSize", config.maxPlatoonSize, config.maxPlatoonSize); - pnh.param("algorithmType", config.algorithmType, config.algorithmType); - pnh.param("statusMessageInterval", config.statusMessageInterval, config.statusMessageInterval); - pnh.param("infoMessageInterval", config.infoMessageInterval, config.infoMessageInterval); - pnh.param("timeHeadway", config.timeHeadway, config.timeHeadway); - pnh.param("standStillHeadway", config.standStillHeadway, config.standStillHeadway); - pnh.param("maxAllowedJoinTimeGap", config.maxAllowedJoinTimeGap, config.maxAllowedJoinTimeGap); - pnh.param("maxAllowedJoinGap", config.maxAllowedJoinGap, config.maxAllowedJoinGap); - pnh.param("minAllowedJoinGap", config.minAllowedJoinGap, config.minAllowedJoinGap); - pnh.param("desiredJoinTimeGap", config.desiredJoinTimeGap, config.desiredJoinTimeGap); - pnh.param("desiredJoinGap", config.desiredJoinGap, config.desiredJoinGap); - pnh.param("waitingStateTimeout", config.waitingStateTimeout, config.waitingStateTimeout); - pnh.param("cmdSpeedMaxAdjustment", config.cmdSpeedMaxAdjustment, config.cmdSpeedMaxAdjustment); - pnh.param("minAllowableHeadaway", config.minAllowableHeadaway, config.minAllowableHeadaway); - pnh.param("maxAllowableHeadaway", config.maxAllowableHeadaway, config.maxAllowableHeadaway); - pnh.param("headawayStableLowerBond", config.headawayStableLowerBond, config.headawayStableLowerBond); - pnh.param("headawayStableUpperBond", config.headawayStableUpperBond, config.headawayStableUpperBond); - pnh.param("minCutinGap", config.minCutinGap, config.minCutinGap); - pnh.param("maxCutinGap", config.maxCutinGap, config.maxCutinGap); - pnh.param("maxCrosstrackError", config.maxCrosstrackError, config.maxCrosstrackError); - pnh.param("test_front_join", config.test_front_join, config.test_front_join); - pnh.param("minPlatooningSpeed", config.minPlatooningSpeed, config.minPlatooningSpeed); - pnh.param("allowCutinJoin", config.allowCutinJoin, config.allowCutinJoin); - pnh.param("significantDTDchange", config.significantDTDchange, config.significantDTDchange); - pnh.param("longitudinalCheckThresold", config.longitudinalCheckThresold, config.longitudinalCheckThresold); - pnh.param("test_cutin_join", config.test_cutin_join, config.test_cutin_join); - pnh.param("join_index", config.join_index, config.join_index); - pnh.getParam("/vehicle_length", config.vehicleLength); - pnh.getParam("/vehicle_id", config.vehicleID); - - ROS_INFO_STREAM("PlatoonPluginConfig Params" << config); - - PlatoonStrategicIHPPlugin worker(wm_, config, [&discovery_pub](auto msg) { discovery_pub.publish(msg); }, [&mob_response_pub](auto msg) { mob_response_pub.publish(msg); }, - [&mob_request_pub](auto msg) { mob_request_pub.publish(msg); }, [&mob_operation_pub](auto msg) { mob_operation_pub.publish(msg); }, - [&platoon_info_pub](auto msg) { platoon_info_pub.publish(msg); } ); - - ros::ServiceServer maneuver_srv_ = nh.advertiseService("platoon_strategic_ihp/plan_maneuvers", - &PlatoonStrategicIHPPlugin::plan_maneuver_cb, &worker); - ros::Subscriber mob_request_sub = nh.subscribe("incoming_mobility_request", 1, &PlatoonStrategicIHPPlugin::mob_req_cb, &worker); - ros::Subscriber mob_response_sub = nh.subscribe("incoming_mobility_response", 1, &PlatoonStrategicIHPPlugin::mob_resp_cb, &worker); - ros::Subscriber mob_operation_sub = nh.subscribe("incoming_mobility_operation", 1, &PlatoonStrategicIHPPlugin::mob_op_cb, &worker); - ros::Subscriber current_pose_sub = nh.subscribe("current_pose", 1, &PlatoonStrategicIHPPlugin::pose_cb, &worker); - ros::Subscriber current_twist_sub = nh.subscribe("current_velocity", 1, &PlatoonStrategicIHPPlugin::twist_cb, &worker); - // not use BSMID, consider delete - // ros::Subscriber bsm_sub = nh.subscribe("bsm_outbound", 1, &PlatoonStrategicIHPPlugin::bsm_cb, &worker); - ros::Subscriber cmd_sub = nh.subscribe("twist_raw", 1, &PlatoonStrategicIHPPlugin::cmd_cb, &worker); - ros::Subscriber georeference_sub = nh.subscribe("georeference", 1, &PlatoonStrategicIHPPlugin::georeference_cb, &worker); - - - ros::Timer discovery_pub_timer_ = nh.createTimer( - ros::Duration(ros::Rate(10.0)), - [&worker](const auto&) { worker.onSpin(); }); - - ros::CARMANodeHandle::spin(); - } -}; - -} // namespace platoon_strategicIHP diff --git a/platooning_strategic_IHP/launch/platoon_strategic_ihp.launch b/platooning_strategic_IHP/launch/platoon_strategic_ihp.launch deleted file mode 100644 index bb9dfa400c..0000000000 --- a/platooning_strategic_IHP/launch/platoon_strategic_ihp.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - diff --git a/platooning_strategic_IHP/launch/platoon_strategic_ihp_launch.py b/platooning_strategic_IHP/launch/platoon_strategic_ihp_launch.py new file mode 100644 index 0000000000..052324c304 --- /dev/null +++ b/platooning_strategic_IHP/launch/platoon_strategic_ihp_launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA platoon_strategic_ihp_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='platoon_strategic_ihp_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='platoon_strategic_ihp', + plugin='platoon_strategic_ihp::Node', + name='platoon_strategic_ihp_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/platooning_strategic_IHP/package.xml b/platooning_strategic_IHP/package.xml index 9e7f8ac83c..bea7bb2444 100644 --- a/platooning_strategic_IHP/package.xml +++ b/platooning_strategic_IHP/package.xml @@ -1,21 +1,53 @@ + + + platoon_strategic_ihp - 3.3.0 - The node is to plan a platooning maneuver - carma - Apache 2.0 License - catkin - carma_utils - cav_msgs - roscpp + 4.0.0 + The platoon_strategic_ihp package + + carma + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components std_msgs - carma_wm - cav_srvs + carma_guidance_plugins + carma_planning_msgs + carma_v2x_msgs + carma_wm_ros2 + lanelet2_core lanelet2_extension tf tf2 tf2_ros tf2_geometry_msgs - carma_cmake_common + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + diff --git a/platooning_strategic_IHP/src/main_ihp.cpp b/platooning_strategic_IHP/src/main.cpp similarity index 56% rename from platooning_strategic_IHP/src/main_ihp.cpp rename to platooning_strategic_IHP/src/main.cpp index fecada04de..f802606e45 100644 --- a/platooning_strategic_IHP/src/main_ihp.cpp +++ b/platooning_strategic_IHP/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -14,20 +14,20 @@ * the License. */ -/* - * Developed by the UCLA Mobility Lab, 10/20/2021. - * - * Creator: Xu Han - * Author: Xu Han, Xin Xia, Jiaqi Ma - */ - -#include -#include "platoon_strategic_plugin_node_ihp.h" +#include +#include "platoon_strategic_ihp/platoon_strategic_plugin_node_ihp.h" -int main(int argc, char** argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "platoon_strategic_ihp"); - platoon_strategic_ihp::PlatoonStrategicIHPPluginNode node; - node.run(); - return 0; + 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/platooning_strategic_IHP/src/platoon_manager_ihp.cpp b/platooning_strategic_IHP/src/platoon_manager_ihp.cpp index 89330be577..0669b43dff 100644 --- a/platooning_strategic_IHP/src/platoon_manager_ihp.cpp +++ b/platooning_strategic_IHP/src/platoon_manager_ihp.cpp @@ -21,10 +21,10 @@ * Author: Xu Han, Xin Xia, Jiaqi Ma */ -#include "platoon_manager_ihp.h" -#include "platoon_config_ihp.h" +#include "platoon_strategic_ihp/platoon_manager_ihp.h" +#include "platoon_strategic_ihp/platoon_config_ihp.h" #include -#include +#include #include namespace platoon_strategic_ihp @@ -48,16 +48,16 @@ namespace platoon_strategic_ihp * */ - PlatoonManager::PlatoonManager() + PlatoonManager::PlatoonManager(std::shared_ptr timer_factory) : timer_factory_(std::move(timer_factory)) { - ROS_DEBUG_STREAM("Top of PlatoonManager ctor."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Top of PlatoonManager ctor."); } // Update the location of the host in the vector of platoon members void PlatoonManager::updateHostPose(const double downtrack, const double crosstrack) { - ROS_DEBUG_STREAM("Host (index " << hostPosInPlatoon_ << "): downtrack = " << downtrack << ", crosstrack = " << crosstrack); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host (index " << hostPosInPlatoon_ << "): downtrack = " << downtrack << ", crosstrack = " << crosstrack); host_platoon_[hostPosInPlatoon_].vehiclePosition = downtrack; host_platoon_[hostPosInPlatoon_].vehicleCrossTrack = crosstrack; } @@ -98,7 +98,7 @@ namespace platoon_strategic_ihp // Sanity check if (platoonLeaderID.compare(host_platoon_[0].staticId) != 0) { - ROS_DEBUG_STREAM("///// platoonLeaderID NOT PROPERLY ASSIGNED! Value = " << platoonLeaderID + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "///// platoonLeaderID NOT PROPERLY ASSIGNED! Value = " << platoonLeaderID << ", host_platoon_[0].staticId = " << host_platoon_[0].staticId); } @@ -111,36 +111,36 @@ namespace platoon_strategic_ihp // TODO: better to have leader explicitly inform us that it is merging into another platoon, rather than require us to deduce it here. // This condition may also result from missing some message traffic, new leader in this platoon, or other confusion/incorrect code. // Consider using a new STATUS msg param that tells members of new platoon ID and new leader ID when a change is made. - ROS_DEBUG_STREAM("It seems that the current leader is joining another platoon."); - ROS_DEBUG_STREAM("So the platoon ID is changed from " << currentPlatoonID << " to " << platoonId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "It seems that the current leader is joining another platoon."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "So the platoon ID is changed from " << currentPlatoonID << " to " << platoonId); currentPlatoonID = platoonId; updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); } else if (currentPlatoonID == platoonId) { - ROS_DEBUG_STREAM("This STATUS messages is from our platoon. Updating the info..."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS messages is from our platoon. Updating the info..."); updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); - ROS_DEBUG_STREAM("The first vehicle in our list is now " << host_platoon_[0].staticId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The first vehicle in our list is now " << host_platoon_[0].staticId); } else //sender is in a different platoon { - ROS_DEBUG_STREAM("This STATUS message is not from a vehicle we care about. Ignore this message with id: " << senderId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS message is not from a vehicle we care about. Ignore this message with id: " << senderId); } } else //host is leader { // If we are currently in any leader state, we only update platoon member based on platoon ID - ROS_DEBUG_STREAM("Host is leader: currentPlatoonID = " << currentPlatoonID << ", incoming platoonId = " << platoonId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host is leader: currentPlatoonID = " << currentPlatoonID << ", incoming platoonId = " << platoonId); if (currentPlatoonID == platoonId) { - ROS_DEBUG_STREAM("This STATUS messages is from our platoon. Updating the info..."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS messages is from our platoon. Updating the info..."); updatesOrAddMemberInfo(host_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); } else { - ROS_DEBUG_STREAM("Platoon IDs not matched"); - ROS_DEBUG_STREAM("currentPlatoonID: " << currentPlatoonID); - ROS_DEBUG_STREAM("incoming platoonId: " << platoonId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Platoon IDs not matched"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "currentPlatoonID: " << currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "incoming platoonId: " << platoonId); } } @@ -177,20 +177,20 @@ namespace platoon_strategic_ihp if (neighborPlatoonID == platoonId) { updatesOrAddMemberInfo(neighbor_platoon_, senderId, cmdSpeed, dtDistance, ctDistance, curSpeed); - ROS_DEBUG_STREAM("This STATUS messages is from the target platoon. Updating the info..."); - ROS_DEBUG_STREAM("The first vehicle in that platoon is now " << neighbor_platoon_[0].staticId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS messages is from the target platoon. Updating the info..."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The first vehicle in that platoon is now " << neighbor_platoon_[0].staticId); // If we have data on all members of a neighboring platoon, set a complete record flag if (neighbor_platoon_info_size_ > 1 && neighbor_platoon_.size() == neighbor_platoon_info_size_) { is_neighbor_record_complete_ = true; - ROS_DEBUG_STREAM("Neighbor record is complete!"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor record is complete!"); } } else //sender is in a different platoon { - ROS_DEBUG_STREAM("This STATUS message is not from a vehicle we care about. Ignore this message from sender: " + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "This STATUS message is not from a vehicle we care about. Ignore this message from sender: " << senderId << " about platoon: " << platoonId); } } @@ -207,7 +207,7 @@ namespace platoon_strategic_ihp if(platoon[i].staticId == senderId) { if (abs(dtDistance - platoon[i].vehiclePosition)/(platoon[i].vehiclePosition + 0.01) > config_.significantDTDchange) { - ROS_DEBUG_STREAM( "DTD of member " << platoon[i].staticId << " is changed significantly, so a new sort is needed"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "DTD of member " << platoon[i].staticId << " is changed significantly, so a new sort is needed"); sortNeeded = true; } @@ -215,17 +215,17 @@ namespace platoon_strategic_ihp platoon[i].vehiclePosition = dtDistance; // m platoon[i].vehicleCrossTrack = ctDistance; // m platoon[i].vehicleSpeed = curSpeed; // m/s - platoon[i].timestamp = ros::Time::now().toNSec()/1000000; - ROS_DEBUG_STREAM("Receive and update platooning info on member " << i << ", ID:" << platoon[i].staticId); - ROS_DEBUG_STREAM(" CommandSpeed = " << platoon[i].commandSpeed); - ROS_DEBUG_STREAM(" Actual Speed = " << platoon[i].vehicleSpeed); - ROS_DEBUG_STREAM(" Downtrack Location = " << platoon[i].vehiclePosition); - ROS_DEBUG_STREAM(" Crosstrack dist = " << platoon[i].vehicleCrossTrack); + platoon[i].timestamp = timer_factory_->now().nanoseconds()/1000000; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Receive and update platooning info on member " << i << ", ID:" << platoon[i].staticId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " CommandSpeed = " << platoon[i].commandSpeed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " Actual Speed = " << platoon[i].vehicleSpeed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " Downtrack Location = " << platoon[i].vehiclePosition); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " Crosstrack dist = " << platoon[i].vehicleCrossTrack); if (senderId == HostMobilityId) { hostPosInPlatoon_ = i; - ROS_DEBUG_STREAM(" This is the HOST vehicle"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " This is the HOST vehicle"); } isExisted = true; } @@ -235,8 +235,8 @@ namespace platoon_strategic_ihp { // sort the platoon member based on dowtrack distance (m) in an descending order. std::sort(std::begin(platoon), std::end(platoon), [](const PlatoonMember &a, const PlatoonMember &b){return a.vehiclePosition > b.vehiclePosition;}); - ROS_DEBUG_STREAM("Platoon is re-sorted due to large difference in dtd update."); - ROS_DEBUG_STREAM(" Platoon order is now:"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Platoon is re-sorted due to large difference in dtd update."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " Platoon order is now:"); for (size_t i = 0; i < platoon.size(); ++i) { std::string hostFlag = " "; @@ -245,21 +245,21 @@ namespace platoon_strategic_ihp hostPosInPlatoon_ = i; hostFlag = "Host"; } - ROS_DEBUG_STREAM(" " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag); } } // if not already exist, add to platoon list. if(!isExisted) { - long cur_t = ros::Time::now().toNSec()/1000000; // time in millisecond + long cur_t = timer_factory_->now().nanoseconds()/1000000; // time in millisecond PlatoonMember newMember = PlatoonMember(senderId, cmdSpeed, curSpeed, dtDistance, ctDistance, cur_t); platoon.push_back(newMember); - // sort the platoon member based on dowtrack distance (m) in an descending order. + // sort the platoon member based on downtrack distance (m) in an descending order. std::sort(std::begin(platoon), std::end(platoon), [](const PlatoonMember &a, const PlatoonMember &b){return a.vehiclePosition > b.vehiclePosition;}); - ROS_DEBUG_STREAM("Add a new vehicle into our platoon list " << newMember.staticId << " platoon.size now = " << platoon.size()); - ROS_DEBUG_STREAM(" Platoon order is now:"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Add a new vehicle into our platoon list " << newMember.staticId << " platoon.size now = " << platoon.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " Platoon order is now:"); for (size_t i = 0; i < platoon.size(); ++i) { std::string hostFlag = " "; @@ -268,7 +268,7 @@ namespace platoon_strategic_ihp hostPosInPlatoon_ = i; hostFlag = "Host"; } - ROS_DEBUG_STREAM(" " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " " << platoon[i].staticId << "its DTD: " << platoon[i].vehiclePosition << " " << hostFlag); } } } @@ -277,7 +277,7 @@ namespace platoon_strategic_ihp // Get the platoon size. int PlatoonManager::getHostPlatoonSize() { - ROS_DEBUG_STREAM("host platoon size: " << host_platoon_.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "host platoon size: " << host_platoon_.size()); return host_platoon_.size(); } @@ -387,10 +387,10 @@ namespace platoon_strategic_ihp // Return the dynamic leader (i.e., the vehicle to follow) of the host vehicle. PlatoonMember PlatoonManager::getDynamicLeader(){ PlatoonMember dynamicLeader; - ROS_DEBUG_STREAM("host_platoon_ size: " << host_platoon_.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "host_platoon_ size: " << host_platoon_.size()); if(isFollower) { - ROS_DEBUG_STREAM("Leader initially set as first vehicle in platoon"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Leader initially set as first vehicle in platoon"); // return the first vehicle in the platoon as default if no valid algorithm applied // due to downtrack descending order, the platoon front veihcle is the first in list. dynamicLeader = host_platoon_[0]; @@ -398,16 +398,16 @@ namespace platoon_strategic_ihp size_t newLeaderIndex = allPredecessorFollowing(); dynamic_leader_index_ = (int)newLeaderIndex; - ROS_DEBUG_STREAM("dynamic_leader_index_: " << dynamic_leader_index_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dynamic_leader_index_: " << dynamic_leader_index_); if(newLeaderIndex < host_platoon_.size()) { //this must always be true! dynamicLeader = host_platoon_[newLeaderIndex]; - ROS_DEBUG_STREAM("APF output: " << dynamicLeader.staticId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF output: " << dynamicLeader.staticId); previousFunctionalDynamicLeaderIndex_ = newLeaderIndex; previousFunctionalDynamicLeaderID_ = dynamicLeader.staticId; } else //something is terribly wrong in the logic! { - ROS_WARN_STREAM("newLeaderIndex = " << newLeaderIndex << " is invalid coming from allPredecessorFollowing!"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "newLeaderIndex = " << newLeaderIndex << " is invalid coming from allPredecessorFollowing!"); /** * it might happened when the subject vehicle gets far away from the preceding vehicle, * in which case the host vehicle will follow the one in front. @@ -416,7 +416,7 @@ namespace platoon_strategic_ihp // update index and ID previousFunctionalDynamicLeaderIndex_ = getNumberOfVehicleInFront()-1; previousFunctionalDynamicLeaderID_ = dynamicLeader.staticId; - ROS_DEBUG_STREAM("Based on the output of APF algorithm we start to follow our predecessor."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Based on the output of APF algorithm we start to follow our predecessor."); } } } @@ -432,13 +432,13 @@ namespace platoon_strategic_ihp if(getNumberOfVehicleInFront() == 1) { // If the host is the second vehicle, then follow the leader. - ROS_DEBUG_STREAM("As the second vehicle in the platoon, it will always follow the leader. Case Zero"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "As the second vehicle in the platoon, it will always follow the leader. Case Zero"); return 0; } ///***** Case One *****/// // If there weren't a leader in the previous time step, follow the first vehicle (i.e., the platoon leader) as default. if(previousFunctionalDynamicLeaderID_ == "") { - ROS_DEBUG_STREAM("APF algorithm did not found a dynamic leader in previous time step. Case one"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF algorithm did not found a dynamic leader in previous time step. Case one"); return 0; } @@ -463,26 +463,26 @@ namespace platoon_strategic_ihp double distHeadwayWithPredecessor = downtrackDistance[hostPosInPlatoon_ - 1] - downtrackDistance[hostPosInPlatoon_]; gapWithPred_ = distHeadwayWithPredecessor; if(insufficientGapWithPredecessor(distHeadwayWithPredecessor)) { - ROS_DEBUG_STREAM("APF algorithm decides there is an issue with the gap with preceding vehicle: " << distHeadwayWithPredecessor << " m. Case Two"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF algorithm decides there is an issue with the gap with preceding vehicle: " << distHeadwayWithPredecessor << " m. Case Two"); return hostPosInPlatoon_ - 1; } else{ // implementation of the main part of APF algorithm // calculate the time headway between every consecutive pair of vehicles std::vector timeHeadways = calculateTimeHeadway(downtrackDistance, speed); - ROS_DEBUG_STREAM("APF calculate time headways: " ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF calculate time headways: " ); for (const auto& value : timeHeadways) { - ROS_DEBUG_STREAM("APF time headways: " << value); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF time headways: " << value); } - ROS_DEBUG_STREAM("APF found the previous dynamic leader is " << previousFunctionalDynamicLeaderID_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found the previous dynamic leader is " << previousFunctionalDynamicLeaderID_); // if the previous dynamic leader is the first vehicle in the platoon if(previousFunctionalDynamicLeaderIndex_ == 0) { ///***** Case Three *****/// // If there is a violation, the return value is the desired dynamic leader index - ROS_DEBUG_STREAM("APF use violations on lower boundary or maximum spacing to choose dynamic leader. Case Three."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF use violations on lower boundary or maximum spacing to choose dynamic leader. Case Three."); return determineDynamicLeaderBasedOnViolation(timeHeadways); } else{ @@ -491,7 +491,7 @@ namespace platoon_strategic_ihp std::vector partialTimeHeadways = getTimeHeadwayFromIndex(timeHeadways, previousFunctionalDynamicLeaderIndex_); for (const auto& value : partialTimeHeadways) { - ROS_DEBUG_STREAM("APF partial time headways: " << value); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF partial time headways: " << value); } int closestLowerBoundaryViolation = findLowerBoundaryViolationClosestToTheHostVehicle(partialTimeHeadways); int closestMaximumSpacingViolation = findMaximumSpacingViolationClosestToTheHostVehicle(partialTimeHeadways); @@ -516,42 +516,42 @@ namespace platoon_strategic_ihp ///***** Case Four *****/// //we may switch dynamic leader further downstream if(condition1 && condition2) { - ROS_DEBUG_STREAM("APF found two conditions for assigning local dynamic leadership further downstream are satisfied. Case Four"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found two conditions for assigning local dynamic leadership further downstream are satisfied. Case Four"); return determineDynamicLeaderBasedOnViolation(timeHeadways); } else { ///***** Case Five *****/// // We may not switch dynamic leadership to another vehicle further downstream because some criteria are not satisfied - ROS_DEBUG_STREAM("APF found two conditions for assigning local dynamic leadership further downstream are not satisfied. Case Five."); - ROS_DEBUG_STREAM("condition1: " << condition1 << " & condition2: " << condition2); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found two conditions for assigning local dynamic leadership further downstream are not satisfied. Case Five."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "condition1: " << condition1 << " & condition2: " << condition2); return previousFunctionalDynamicLeaderIndex_; } } else if(closestLowerBoundaryViolation != -1 && closestMaximumSpacingViolation == -1) { // The rest four cases have roughly the same logic: locate the closest violation and assign dynamic leadership accordingly ///***** Case Six *****/// - ROS_DEBUG_STREAM("APF found closestLowerBoundaryViolation on partial time headways. Case Six."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found closestLowerBoundaryViolation on partial time headways. Case Six."); return previousFunctionalDynamicLeaderIndex_ - 1 + closestLowerBoundaryViolation; } else if(closestLowerBoundaryViolation == -1 && closestMaximumSpacingViolation != -1) { ///***** Case Seven *****/// - ROS_DEBUG_STREAM("APF found closestMaximumSpacingViolation on partial time headways. Case Seven."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found closestMaximumSpacingViolation on partial time headways. Case Seven."); return previousFunctionalDynamicLeaderIndex_ + closestMaximumSpacingViolation; } else{ - ROS_DEBUG_STREAM("APF found closestMaximumSpacingViolation and closestLowerBoundaryViolation on partial time headways."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found closestMaximumSpacingViolation and closestLowerBoundaryViolation on partial time headways."); if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) { ///***** Case Eight *****/// - ROS_DEBUG_STREAM("closest LowerBoundaryViolation is higher than closestMaximumSpacingViolation on partial time headways. Case Eight."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "closest LowerBoundaryViolation is higher than closestMaximumSpacingViolation on partial time headways. Case Eight."); return previousFunctionalDynamicLeaderIndex_ - 1 + closestLowerBoundaryViolation; } else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation) { ///***** Case Nine *****/// - ROS_DEBUG_STREAM("closestMaximumSpacingViolation is higher than closestLowerBoundaryViolation on partial time headways. Case Nine."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "closestMaximumSpacingViolation is higher than closestLowerBoundaryViolation on partial time headways. Case Nine."); return previousFunctionalDynamicLeaderIndex_ + closestMaximumSpacingViolation; } else { - ROS_DEBUG_STREAM("APF dynamic Leader selection parameter is wrong!"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF dynamic Leader selection parameter is wrong!"); return 0; } } @@ -622,15 +622,15 @@ namespace platoon_strategic_ihp // Compare the violation locations, always following the closer violation vehicle (larger index) first, then the furthur ones. if(closestLowerBoundaryViolation > closestMaximumSpacingViolation) { - ROS_DEBUG_STREAM("APF found violation on closestLowerBoundaryViolation at " << closestLowerBoundaryViolation); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found violation on closestLowerBoundaryViolation at " << closestLowerBoundaryViolation); return closestLowerBoundaryViolation; // Min violation, following the vehicle that is in font of the violating gap. } else if(closestLowerBoundaryViolation < closestMaximumSpacingViolation){ - ROS_DEBUG_STREAM("APF found violation on closestMaximumSpacingViolation at " << closestMaximumSpacingViolation); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found violation on closestMaximumSpacingViolation at " << closestMaximumSpacingViolation); return closestMaximumSpacingViolation + 1; // Max violation, follow the vehicle that is behinf the violating gap. } else{ - ROS_DEBUG_STREAM("APF found no violations on both closestLowerBoundaryViolation and closestMaximumSpacingViolation"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "APF found no violations on both closestLowerBoundaryViolation and closestMaximumSpacingViolation"); return 0; } } @@ -680,7 +680,7 @@ namespace platoon_strategic_ihp host_platoon_.erase(host_platoon_.begin(), host_platoon_.begin() + hostPosInPlatoon_); }else { - ROS_WARN("### Host becoming leader, but is already at index 0 in host_platoon_ vector! Vector unchanged."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"), "### Host becoming leader, but is already at index 0 in host_platoon_ vector! Vector unchanged."); } hostPosInPlatoon_ = 0; @@ -688,7 +688,7 @@ namespace platoon_strategic_ihp currentPlatoonID = boost::uuids::to_string(boost::uuids::random_generator()()); previousFunctionalDynamicLeaderID_ = ""; previousFunctionalDynamicLeaderIndex_ = -1; - ROS_DEBUG_STREAM("The platoon manager is changed from follower state to leader state. New platoon ID = " << currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon manager is changed from follower state to leader state. New platoon ID = " << currentPlatoonID); } // Change the local platoon manager from leader operation state to follower operation state for single vehicle status change. @@ -716,7 +716,7 @@ namespace platoon_strategic_ihp // Clear the record of neighbor platoon, since we likely just joined it resetNeighborPlatoon(); - ROS_DEBUG_STREAM("The platoon manager is changed from leader state to follower state. Platoon vector re-initialized. Plan ID = " << newPlatoonId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon manager is changed from leader state to follower state. Platoon vector re-initialized. Plan ID = " << newPlatoonId); } // Return the number of vehicles in the front of the host vehicle. If host is leader or a single vehicle, return 0. @@ -919,15 +919,15 @@ namespace platoon_strategic_ihp double min_diff = 99999.0; int cut_in_index = -1; //-2 is meaningless default - ROS_DEBUG_STREAM("neighbor_platoon_.size(): " << neighbor_platoon_.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "neighbor_platoon_.size(): " << neighbor_platoon_.size()); // Loop through all target platoon members for(size_t i = 0; i < neighbor_platoon_.size(); i++) { double current_member_dtd = neighbor_platoon_[i].vehiclePosition; - ROS_DEBUG_STREAM("current_member_dtd: "<< current_member_dtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_member_dtd: "<< current_member_dtd); double curent_dtd_diff = current_member_dtd - joinerDtD; - ROS_DEBUG_STREAM("curent_dtd_diff: "<< curent_dtd_diff); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "curent_dtd_diff: "<< curent_dtd_diff); // update min index if (curent_dtd_diff > 0 && curent_dtd_diff < min_diff) { diff --git a/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp index a27e697603..db480a4264 100644 --- a/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp +++ b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp @@ -22,35 +22,31 @@ */ -#include +#include #include -#include "platoon_strategic_ihp.h" +#include "platoon_strategic_ihp/platoon_strategic_ihp.h" #include #include namespace platoon_strategic_ihp { - - PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin() - { - } // -------------- constructor --------------// - PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher, MobilityResponseCB mobility_response_publisher, + PlatoonStrategicIHPPlugin::PlatoonStrategicIHPPlugin(carma_wm::WorldModelConstPtr wm, PlatoonPluginConfig config, MobilityResponseCB mobility_response_publisher, MobilityRequestCB mobility_request_publisher, MobilityOperationCB mobility_operation_publisher, - PlatooningInfoCB platooning_info_publisher) - : plugin_discovery_publisher_(plugin_discovery_publisher), mobility_request_publisher_(mobility_request_publisher), + PlatooningInfoCB platooning_info_publisher, + std::shared_ptr timer_factory) + : mobility_request_publisher_(mobility_request_publisher), mobility_response_publisher_(mobility_response_publisher), mobility_operation_publisher_(mobility_operation_publisher), - platooning_info_publisher_(platooning_info_publisher), wm_(wm), config_(config) + platooning_info_publisher_(platooning_info_publisher), wm_(wm), config_(config), timer_factory_(timer_factory), pm_(timer_factory_) { - ROS_DEBUG_STREAM("Top of PlatoonStrategicIHP ctor."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Top of PlatoonStrategicIHP ctor."); std::string hostStaticId = config_.vehicleID; //static ID for this vehicle pm_.HostMobilityId = hostStaticId; // construct platoon member for host vehicle as the first element in the vector, since it starts life as a solo vehicle - long cur_t = ros::Time::now().toNSec()/1000000; // time in millisecond + long cur_t = timer_factory_->now().nanoseconds()/1000000; // time in millisecond PlatoonMember hostVehicleMember = PlatoonMember(hostStaticId, 0.0, 0.0, 0.0, 0.0, cur_t); pm_.host_platoon_.push_back(hostVehicleMember); @@ -58,16 +54,16 @@ namespace platoon_strategic_ihp plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; plugin_discovery_msg_.activated = true; - plugin_discovery_msg_.type = cav_msgs::Plugin::STRATEGIC; + plugin_discovery_msg_.type = carma_planning_msgs::msg::Plugin::STRATEGIC; plugin_discovery_msg_.capability = "strategic_plan/plan_maneuvers"; - ROS_DEBUG_STREAM("ctor complete. hostStaticId = " << hostStaticId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "ctor complete. hostStaticId = " << hostStaticId); } //-------------------------------- Extract Data --------------------------------------// // Find ecef point based on pose message - cav_msgs::LocationECEF PlatoonStrategicIHPPlugin::pose_to_ecef(geometry_msgs::PoseStamped pose_msg) + carma_v2x_msgs::msg::LocationECEF PlatoonStrategicIHPPlugin::pose_to_ecef(geometry_msgs::msg::PoseStamped pose_msg) { if (!map_projector_) @@ -75,7 +71,7 @@ namespace platoon_strategic_ihp throw std::invalid_argument("No map projector available for ecef conversion"); } - cav_msgs::LocationECEF location; + carma_v2x_msgs::msg::LocationECEF location; // note: ecef point read from map projector is in m. lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({pose_msg.pose.position.x, pose_msg.pose.position.y, 0.0}, 1); @@ -84,16 +80,16 @@ namespace platoon_strategic_ihp location.ecef_z = ecef_point.z() * 100.0; - // ROS_DEBUG_STREAM("location.ecef_x: " << location.ecef_x); - // ROS_DEBUG_STREAM("location.ecef_y: " << location.ecef_y); - // ROS_DEBUG_STREAM("location.ecef_z: " << location.ecef_z); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "location.ecef_x: " << location.ecef_x); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "location.ecef_y: " << location.ecef_y); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "location.ecef_z: " << location.ecef_z); // note: the returned ecef is in cm. return location; } // Function to assign host pose_ecef_point_ - void PlatoonStrategicIHPPlugin::setHostECEF(cav_msgs::LocationECEF pose_ecef_point) + void PlatoonStrategicIHPPlugin::setHostECEF(carma_v2x_msgs::msg::LocationECEF pose_ecef_point) { // Note, the ecef here is in cm. pose_ecef_point_ = pose_ecef_point; @@ -118,9 +114,9 @@ namespace platoon_strategic_ihp } // Callback to calculate downtrack based on pose message. - void PlatoonStrategicIHPPlugin::pose_cb(const geometry_msgs::PoseStampedConstPtr& msg) + void PlatoonStrategicIHPPlugin::pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg) { - pose_msg_ = geometry_msgs::PoseStamped(*msg.get()); + pose_msg_ = geometry_msgs::msg::PoseStamped(*msg); if (pm_.current_platoon_state != PlatoonState::STANDBY) { @@ -130,23 +126,23 @@ namespace platoon_strategic_ihp // update host's DtD and CtD current_downtrack_ = tc.downtrack; current_crosstrack_ = tc.crosstrack; - // ROS_DEBUG_STREAM("current_downtrack_ = " << current_downtrack_ << ", current_crosstrack_ = " << current_crosstrack_); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_downtrack_ = " << current_downtrack_ << ", current_crosstrack_ = " << current_crosstrack_); pm_.updateHostPose(current_downtrack_, current_crosstrack_); // note: the ecef read from "pose_ecef_point" is in cm. - cav_msgs::LocationECEF pose_ecef_point = pose_to_ecef(pose_msg_); + carma_v2x_msgs::msg::LocationECEF pose_ecef_point = pose_to_ecef(pose_msg_); setHostECEF(pose_ecef_point); } } // callback kto update the command speed on x direction, in m/s. - void PlatoonStrategicIHPPlugin::cmd_cb(const geometry_msgs::TwistStampedConstPtr& msg) + void PlatoonStrategicIHPPlugin::cmd_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg) { cmd_speed_ = msg->twist.linear.x; } // twist command, linear speed on x direction, in m/s. - void PlatoonStrategicIHPPlugin::twist_cb(const geometry_msgs::TwistStampedConstPtr& msg) + void PlatoonStrategicIHPPlugin::twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg) { current_speed_ = msg->twist.linear.x; if (current_speed_ < STOPPED_SPEED) @@ -183,7 +179,7 @@ namespace platoon_strategic_ihp throw std::invalid_argument("Valid traffic rules object could not be built"); } - ROS_DEBUG_STREAM("target speed (limit) " << target_speed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target speed (limit) " << target_speed); return target_speed; } @@ -204,7 +200,7 @@ namespace platoon_strategic_ihp double dx = abs (left_bound[0].x() - right_bound[0].x()); double dy = abs (left_bound[0].y() - right_bound[0].y()); double laneWidth = sqrt(dx*dx + dy*dy); - ROS_DEBUG_STREAM("calculated lane width: " << laneWidth); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "calculated lane width: " << laneWidth); // TODO temporary disable this function and return constant value laneWidth = 3.5; @@ -223,13 +219,13 @@ namespace platoon_strategic_ihp if (downtrack > currentDtd && samelane) { - ROS_DEBUG_STREAM("Found a platoon in front. We are able to join"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a platoon in front. We are able to join"); return true; } else { - ROS_DEBUG_STREAM("Ignoring platoon that is either behind host or in another lane."); - ROS_DEBUG_STREAM("The front platoon dtd is " << downtrack << " and we are current at " << currentDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring platoon that is either behind host or in another lane."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The front platoon dtd is " << downtrack << " and we are current at " << currentDtd); return false; } } @@ -243,19 +239,19 @@ namespace platoon_strategic_ihp if (downtrack < currentDtd && samelane) { - ROS_DEBUG_STREAM("Found a platoon at behind. We are able to join"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a platoon at behind. We are able to join"); return true; } else { - ROS_DEBUG_STREAM("Ignoring platoon that is either ahead of us or in another lane."); - ROS_DEBUG_STREAM("The front platoon dtd is " << downtrack << " and we are current at " << currentDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring platoon that is either ahead of us or in another lane."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The front platoon dtd is " << downtrack << " and we are current at " << currentDtd); return false; } } // Return the ecef point projected to local map point. - lanelet::BasicPoint2d PlatoonStrategicIHPPlugin::ecef_to_map_point(cav_msgs::LocationECEF ecef_point) + lanelet::BasicPoint2d PlatoonStrategicIHPPlugin::ecef_to_map_point(carma_v2x_msgs::msg::LocationECEF ecef_point) { if (!map_projector_) { @@ -269,7 +265,7 @@ namespace platoon_strategic_ihp } // Build map projector from proj string (georefernce). - void PlatoonStrategicIHPPlugin::georeference_cb(const std_msgs::StringConstPtr& msg) + void PlatoonStrategicIHPPlugin::georeference_cb(const std_msgs::msg::String::UniquePtr msg) { map_projector_ = std::make_shared(msg->data.c_str()); } @@ -280,7 +276,7 @@ namespace platoon_strategic_ihp // ------ 1. compose Mobility Operation messages and platoon info ------ // // UCLA: Return a Mobility operation message with STATUS params. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationSTATUS() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationSTATUS() { /** * Note: STATUS params format: @@ -292,12 +288,12 @@ namespace platoon_strategic_ihp // This requires an architectural agreement on use of group messaging protocol. // Extract data - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg.m_header.plan_id = pm_.currentPlatoonID; msg.m_header.recipient_id = ""; std::string hostStaticId = config_.vehicleID; msg.m_header.sender_id = hostStaticId; - msg.m_header.timestamp = ros::Time::now().toNSec() / 1000000; + msg.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000; msg.strategy = PLATOONING_STRATEGY; // form message @@ -312,24 +308,24 @@ namespace platoon_strategic_ihp // compose message std::string statusParams = fmter.str(); msg.strategy_params = statusParams; - ROS_DEBUG_STREAM("Composed a mobility operation message with params " << msg.strategy_params); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Composed a mobility operation message with params " << msg.strategy_params); return msg; } // UCLA: Return a Mobility operation message with INFO params. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationINFO() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationINFO() { /** * Note: INFO param format: * "INFO| --> LENGTH:%.2f,SPEED:%.2f,SIZE:%d,ECEFX:%.2f,ECEFY:%.2f,ECEFZ:%.2f" * |-------0-----------1---------2--------3----------4----------5-------| */ - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg.m_header.plan_id = pm_.currentPlatoonID; // msg.m_header.plan_id is the platoon ID of the request sender (rear join and frontal join). msg.m_header.recipient_id = ""; std::string hostStaticId = config_.vehicleID; msg.m_header.sender_id = hostStaticId; - msg.m_header.timestamp = ros::Time::now().toNSec() / 1000000;; + msg.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;; msg.strategy = PLATOONING_STRATEGY; double CurrentPlatoonLength = pm_.getCurrentPlatoonLength(); @@ -346,7 +342,7 @@ namespace platoon_strategic_ihp std::string infoParams = fmter.str(); msg.strategy_params = infoParams; - ROS_DEBUG_STREAM("Composed a mobility operation message with params " << msg.strategy_params); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Composed a mobility operation message with params " << msg.strategy_params); return msg; } @@ -373,19 +369,19 @@ namespace platoon_strategic_ihp bool lateralCheck = joining_crosstrack >= frontVehicleCtd - two_lane_cross_error || joining_crosstrack <= frontVehicleCtd + two_lane_cross_error; // logs for longitudinal and lateral check - ROS_DEBUG_STREAM("The longitudinalCheck result is: " << longitudinalCheck ); - ROS_DEBUG_STREAM("The lateralCheck result is: " << lateralCheck ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The longitudinalCheck result is: " << longitudinalCheck ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The lateralCheck result is: " << lateralCheck ); if (longitudinalCheck && lateralCheck) { - ROS_DEBUG_STREAM("Joining vehicle is nearby. It is able to join."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining vehicle is nearby. It is able to join."); return true; } else { - ROS_DEBUG_STREAM("The joining vehicle is not close by, the join request will not be approved."); - ROS_DEBUG_STREAM("The joining vehicle downtrack is " << joining_downtrack << " and the host (platoon leader) downtrack is " << frontVehicleDtd); - ROS_DEBUG_STREAM("The joining vehicle crosstrack is " << joining_crosstrack << " and the host (platoon leader) crosstrack is " << frontVehicleCtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is not close by, the join request will not be approved."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle downtrack is " << joining_downtrack << " and the host (platoon leader) downtrack is " << frontVehicleDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle crosstrack is " << joining_crosstrack << " and the host (platoon leader) crosstrack is " << frontVehicleCtd); return false; } } @@ -403,26 +399,26 @@ namespace platoon_strategic_ihp // current_crosstrack_ >= frontVehicleCtd - two_lane_cross_error || // current_crosstrack_ <= frontVehicleCtd + two_lane_cross_error; // logs for longitudinal and lateral check - ROS_DEBUG_STREAM("The longitudinalCheck result is: " << longitudinalCheck ); - ROS_DEBUG_STREAM("The lateralCheck result is: " << lateralCheck ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The longitudinalCheck result is: " << longitudinalCheck ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The lateralCheck result is: " << lateralCheck ); if (longitudinalCheck && lateralCheck) { // host vehicle is close to target platoon longitudinally (within 10m) and laterally (within 5m) - ROS_DEBUG_STREAM("Found a platoon nearby. We are able to join."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a platoon nearby. We are able to join."); return true; } else { - ROS_DEBUG_STREAM("Ignoring platoon."); - ROS_DEBUG_STREAM("The platoon leader dtd is " << frontVehicleDtd << " and we are current at " << current_downtrack_); - ROS_DEBUG_STREAM("The platoon leader ctd is " << frontVehicleCtd << " and we are current at " << current_crosstrack_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring platoon."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon leader dtd is " << frontVehicleDtd << " and we are current at " << current_downtrack_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The platoon leader ctd is " << frontVehicleCtd << " and we are current at " << current_crosstrack_); return false; } } // Compose platoon info msg for all states. - cav_msgs::PlatooningInfo PlatoonStrategicIHPPlugin::composePlatoonInfoMsg() + carma_planning_msgs::msg::PlatooningInfo PlatoonStrategicIHPPlugin::composePlatoonInfoMsg() { /** * Note: There is a difference between the "platoon info status" versus the the "platoon strategic plugin states". @@ -431,47 +427,47 @@ namespace platoon_strategic_ihp * A more detailed note can be found in the corresponding function declaration in "platoon_strategic_ihp.h" file. */ - cav_msgs::PlatooningInfo status_msg; + carma_planning_msgs::msg::PlatooningInfo status_msg; if (pm_.current_platoon_state == PlatoonState::STANDBY) { - status_msg.state = cav_msgs::PlatooningInfo::DISABLED; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::DISABLED; } else if (pm_.current_platoon_state == PlatoonState::LEADER) { - status_msg.state = pm_.getHostPlatoonSize() == 1 ? cav_msgs::PlatooningInfo::SEARCHING : cav_msgs::PlatooningInfo::LEADING; + status_msg.state = pm_.getHostPlatoonSize() == 1 ? carma_planning_msgs::msg::PlatooningInfo::SEARCHING : carma_planning_msgs::msg::PlatooningInfo::LEADING; } else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; } else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_LEADER; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_LEADER; } else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) { - status_msg.state = cav_msgs::PlatooningInfo::FOLLOWING; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::FOLLOWING; } // UCLA: add leader aborting for frontal join (inherited from candidate follower). else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_LEADER; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_LEADER; } // UCLA: add candidate leader for frontal join (inherited from leader waiting). else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; } // UCLA: add "lead with operation" for frontal join (inherited from leader waiting). else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_FOLLOWER; } // UCLA: add "prepare to join" for frontal join (inherited from leader waiting). else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) { - status_msg.state = cav_msgs::PlatooningInfo::CONNECTING_TO_NEW_LEADER; + status_msg.state = carma_planning_msgs::msg::PlatooningInfo::CONNECTING_TO_NEW_LEADER; } //TODO: Place holder for departure (PREPARE TO DEPART) @@ -489,10 +485,10 @@ namespace platoon_strategic_ihp status_msg.leader_downtrack_distance = platoon_leader.vehiclePosition; status_msg.leader_cmd_speed = platoon_leader.commandSpeed; status_msg.host_platoon_position = pm_.getNumberOfVehicleInFront(); - ROS_DEBUG_STREAM("pm platoonsize: " << pm_.getHostPlatoonSize() << ", platoon_leader " << platoon_leader.staticId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm platoonsize: " << pm_.getHostPlatoonSize() << ", platoon_leader " << platoon_leader.staticId); int numOfVehiclesGaps = pm_.getNumberOfVehicleInFront() - pm_.dynamic_leader_index_; - ROS_DEBUG_STREAM("The host vehicle have " << numOfVehiclesGaps << " vehicles between itself and its leader (includes the leader)"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The host vehicle have " << numOfVehiclesGaps << " vehicles between itself and its leader (includes the leader)"); // use current position to find lanelet ID lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y); @@ -513,16 +509,16 @@ namespace platoon_strategic_ihp else { - ROS_DEBUG_STREAM("No lanelets in this location!!!: "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "No lanelets in this location!!!: "); } - ROS_DEBUG_STREAM("lanelet_digitalgap: " << lanelet_digitalgap); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lanelet_digitalgap: " << lanelet_digitalgap); double desired_headway = std::max(current_speed_ * config_.timeHeadway, lanelet_digitalgap); - ROS_DEBUG_STREAM("speed based gap: " << current_speed_ * config_.timeHeadway); - ROS_DEBUG_STREAM("max desired_headway " << desired_headway); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed based gap: " << current_speed_ * config_.timeHeadway); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "max desired_headway " << desired_headway); // TODO: currently the average length of the vehicle is obtained from a config parameter. In future, plugin needs to be updated to receive each vehicle's actual length through status or BSM messages for more accuracy. status_msg.desired_gap = std::max(config_.standStillHeadway * numOfVehiclesGaps, desired_headway * numOfVehiclesGaps) + (numOfVehiclesGaps - 1) * 5.0;//config_.averageVehicleLength; - ROS_DEBUG_STREAM("The desired gap with the leader is " << status_msg.desired_gap); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The desired gap with the leader is " << status_msg.desired_gap); // TODO: To uncomment the following lines, platooninfo msg must be updated @@ -534,15 +530,15 @@ namespace platoon_strategic_ihp // Note: use isCreateGap to adjust the desired gap send to control plugin double regular_gap = status_msg.desired_gap; - ROS_DEBUG_STREAM("regular_gap: " << regular_gap); - ROS_DEBUG_STREAM("current_speed_: " << current_speed_); - ROS_DEBUG_STREAM("speed based gap: " << desired_headway); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "regular_gap: " << regular_gap); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_speed_: " << current_speed_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed based gap: " << desired_headway); if (pm_.isCreateGap){ // enlarged desired gap for gap creation status_msg.desired_gap = regular_gap*(1 + config_.createGapAdjuster); } status_msg.actual_gap = platoon_leader.vehiclePosition - current_downtrack_; - ROS_DEBUG_STREAM("status_msg.actual_gap: " << status_msg.actual_gap); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "status_msg.actual_gap: " << status_msg.actual_gap); } else { @@ -561,9 +557,9 @@ namespace platoon_strategic_ihp } // Compose the Mobility Operation message for leader state. Message parameter types: STATUS and INFO. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeader(const std::string& type) + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeader(const std::string& type) { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; // info params if (type == OPERATION_INFO_TYPE) @@ -579,79 +575,79 @@ namespace platoon_strategic_ihp // Unknown strategy param. else { - ROS_ERROR("UNKNOWN strategy param string!!!"); + RCLCPP_ERROR(rclcpp::get_logger("platoon_strategic_ihp"),"UNKNOWN strategy param string!!!"); msg.strategy_params = ""; } return msg; } // Compose the Mobility Operation message for Follower state. Message parameter types: STATUS. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationFollower() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationFollower() { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg = composeMobilityOperationSTATUS(); return msg; } // Compose the Mobility Operation message for LeaderWaiting state. Message parameter types: STATUS. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderWaiting() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderWaiting() { //TODO: shouldn't a leaderwaiting also be sending INFO messages since it is still leading? - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg = composeMobilityOperationSTATUS(); return msg; } // Compose the Mobility Operation message for CandidateFollower state. Message parameter types: STATUS. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateFollower() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateFollower() { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg = composeMobilityOperationSTATUS(); return msg; } // UCLA: add compose msgs for LeaderAborting (inherited from candidate follower). Message parameter types: STATUS. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderAborting() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeaderAborting() { /* UCLA Implementation note: Sending STATUS info for member updates and platoon trajectory regulation. */ - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg = composeMobilityOperationSTATUS(); return msg; } // UCLA: add compose msgs for CandidateLeader (inherited from leader waiting). Message parameter types: STATUS. - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateLeader() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationCandidateLeader() { /* UCLA Implementation note: This is the joiner which will later become the new leader, host vehicle publish status msgs and waiting to lead the rear platoon */ - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg = composeMobilityOperationSTATUS(); return msg; } // UCLA: compose mobility message for prepare to join (cut-in join state, inherited from follower state's compose mob_op) - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationPrepareToJoin() + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationPrepareToJoin() { /* UCLA Implementation note: This is the joiner that is preapring for cut-in join. host vehicle publish status msgs and waiting to lead the rear platoon. */ - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg = composeMobilityOperationSTATUS(); return msg; } // UCLA: compose mobility message for leading with operation (cut-in join state, inherited from leader state's compose mob_op) - cav_msgs::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeadWithOperation(const std::string& type) + carma_v2x_msgs::msg::MobilityOperation PlatoonStrategicIHPPlugin::composeMobilityOperationLeadWithOperation(const std::string& type) { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; // info params if (type == OPERATION_INFO_TYPE) @@ -667,7 +663,7 @@ namespace platoon_strategic_ihp // Unknown strategy param. else { - ROS_ERROR("UNKNOWN strategy param string!!!"); + RCLCPP_ERROR(rclcpp::get_logger("platoon_strategic_ihp"),"UNKNOWN strategy param string!!!"); msg.strategy_params = ""; } return msg; @@ -678,7 +674,7 @@ namespace platoon_strategic_ihp // ------ 2. Mobility operation callback ------ // // read ecef pose from STATUS - cav_msgs::LocationECEF PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_STATUS_params(std::string strategyParams) + carma_v2x_msgs::msg::LocationECEF PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_STATUS_params(std::string strategyParams) { /* * Helper function that extract ecef location from STATUS msg. @@ -702,7 +698,7 @@ namespace platoon_strategic_ihp boost::algorithm::split(ecef_z_parsed, inputsParams[4], boost::is_any_of(":")); double ecef_z = std::stod(ecef_z_parsed[1]); - cav_msgs::LocationECEF ecef_loc; + carma_v2x_msgs::msg::LocationECEF ecef_loc; ecef_loc.ecef_x = ecef_x; ecef_loc.ecef_y = ecef_y; ecef_loc.ecef_z = ecef_z; @@ -711,7 +707,7 @@ namespace platoon_strategic_ihp } // UCLA: Handle STATUS operation messages - void PlatoonStrategicIHPPlugin::mob_op_cb_STATUS(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_STATUS(const carma_v2x_msgs::msg::MobilityOperation& msg) { /** * Note: STATUS params format: @@ -719,28 +715,28 @@ namespace platoon_strategic_ihp * |----------0----------1---------2---------3---------4------| */ - ROS_DEBUG_STREAM("Entered mob_op_cb_STATUS"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Entered mob_op_cb_STATUS"); std::string strategyParams = msg.strategy_params; std::string vehicleID = msg.m_header.sender_id; std::string platoonId = msg.m_header.plan_id; - ROS_DEBUG_STREAM("strategyParams = " << strategyParams); - ROS_DEBUG_STREAM("platoonId = " << platoonId << ", sender ID = " << vehicleID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "strategyParams = " << strategyParams); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "platoonId = " << platoonId << ", sender ID = " << vehicleID); std::string statusParams = strategyParams.substr(OPERATION_STATUS_TYPE.size() + 1); - ROS_DEBUG_STREAM("pm_.currentPlatoonID = " << pm_.currentPlatoonID << ", targetPlatoonID = " << pm_.targetPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID = " << pm_.currentPlatoonID << ", targetPlatoonID = " << pm_.targetPlatoonID); // read Downtrack - cav_msgs::LocationECEF ecef_loc = mob_op_find_ecef_from_STATUS_params(strategyParams); + carma_v2x_msgs::msg::LocationECEF ecef_loc = mob_op_find_ecef_from_STATUS_params(strategyParams); lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); double dtd = wm_->routeTrackPos(incoming_pose).downtrack; - ROS_DEBUG_STREAM("DTD calculated from ecef is: " << dtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "DTD calculated from ecef is: " << dtd); // read Crosstrack double ctd = wm_->routeTrackPos(incoming_pose).crosstrack; - ROS_DEBUG_STREAM("CTD calculated from ecef is: " << ctd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CTD calculated from ecef is: " << ctd); // If it comes from a member of an identified neighbor platoon, then if (platoonId.compare(pm_.neighborPlatoonID) == 0 && platoonId.compare(pm_.dummyID) != 0) { - ROS_DEBUG_STREAM("Incoming platoonID matches target platoon id"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Incoming platoonID matches target platoon id"); // // Update this member's status (or add if it's unknown to us) pm_.neighborMemberUpdates(vehicleID, platoonId, statusParams, dtd, ctd); } @@ -754,7 +750,7 @@ namespace platoon_strategic_ihp // else it represents an uninteresting platoon else { - ROS_DEBUG_STREAM("Received mob op for platoon " << platoonId << " that doesn't match our platoon: " << pm_.currentPlatoonID + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mob op for platoon " << platoonId << " that doesn't match our platoon: " << pm_.currentPlatoonID << " or known neighbor platoon: " << pm_.targetPlatoonID); } } @@ -780,7 +776,7 @@ namespace platoon_strategic_ihp } // UCLA: Parse ecef location from INFO params - cav_msgs::LocationECEF PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_INFO_params(std::string strategyParams) + carma_v2x_msgs::msg::LocationECEF PlatoonStrategicIHPPlugin::mob_op_find_ecef_from_INFO_params(std::string strategyParams) { /** * Note: INFO param format: @@ -803,7 +799,7 @@ namespace platoon_strategic_ihp boost::algorithm::split(ecef_z_parsed, inputsParams[5], boost::is_any_of(":")); double ecef_z = std::stod(ecef_z_parsed[1]); - cav_msgs::LocationECEF ecef_loc; + carma_v2x_msgs::msg::LocationECEF ecef_loc; ecef_loc.ecef_x = ecef_x; ecef_loc.ecef_y = ecef_y; ecef_loc.ecef_z = ecef_z; @@ -812,40 +808,40 @@ namespace platoon_strategic_ihp } // handle message for each states. - void PlatoonStrategicIHPPlugin::mob_op_cb(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb(const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg) { if (pm_.current_platoon_state == PlatoonState::STANDBY) { return; } - ROS_DEBUG_STREAM("mob_op_cb received msg with sender ID " << msg.m_header.sender_id - << ", plan ID " << msg.m_header.plan_id); - ROS_DEBUG_STREAM("...strategy " << msg.strategy << ", strategy params " << msg.strategy_params); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "mob_op_cb received msg with sender ID " << msg->m_header.sender_id + << ", plan ID " << msg->m_header.plan_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "...strategy " << msg->strategy << ", strategy params " << msg->strategy_params); // Check that this is a message about platooning (could be from some other Carma activity nearby) - std::string strategy = msg.strategy; + std::string strategy = msg->strategy; if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0) { - ROS_DEBUG_STREAM("Ignoring mobility operation message for " << strategy << " strategy."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring mobility operation message for " << strategy << " strategy."); return; } // Ignore messages as long as host vehicle is stopped if (current_speed_ < STOPPED_SPEED) { - ROS_DEBUG_STREAM("Ignoring message since host is stopped."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring message since host is stopped."); return; } // Perform common operations that apply to all states - std::string strategyParams = msg.strategy_params; + std::string strategyParams = msg->strategy_params; bool isPlatoonStatusMsg = strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0; bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0; - ROS_DEBUG_STREAM("strategyParams: " << strategyParams << "isPlatoonStatusMsg: " << isPlatoonStatusMsg << "isPlatoonInfoMsg: " << isPlatoonInfoMsg); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "strategyParams: " << strategyParams << "isPlatoonStatusMsg: " << isPlatoonStatusMsg << "isPlatoonInfoMsg: " << isPlatoonInfoMsg); if (isPlatoonStatusMsg) { - mob_op_cb_STATUS(msg); + mob_op_cb_STATUS(*msg); } else if (isPlatoonInfoMsg) { @@ -858,68 +854,68 @@ namespace platoon_strategic_ihp std::vector p_size; boost::algorithm::split(p_size, inputsParams[2], boost::is_any_of(":")); int platoon_size = std::stoi(p_size[1]); - ROS_DEBUG_STREAM("neighbor platoon_size from INFO: " << platoon_size); - if (platoon_size > 1 && msg.m_header.plan_id.compare(pm_.currentPlatoonID) != 0) + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "neighbor platoon_size from INFO: " << platoon_size); + if (platoon_size > 1 && msg->m_header.plan_id.compare(pm_.currentPlatoonID) != 0) { // If platoon ID doesn't match our known target platoon then clear any old neighbor platoon info and record // the platoon ID and the sender as the leader (only leaders send INFO) - if (msg.m_header.plan_id.compare(pm_.neighborPlatoonID) != 0) + if (msg->m_header.plan_id.compare(pm_.neighborPlatoonID) != 0) { pm_.resetNeighborPlatoon(); - pm_.neighborPlatoonID = msg.m_header.plan_id; + pm_.neighborPlatoonID = msg->m_header.plan_id; } - pm_.neighbor_platoon_leader_id_ = msg.m_header.sender_id; - ROS_DEBUG_STREAM("pm_.neighbor_platoon_leader_id_: " << pm_.neighbor_platoon_leader_id_); + pm_.neighbor_platoon_leader_id_ = msg->m_header.sender_id; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.neighbor_platoon_leader_id_: " << pm_.neighbor_platoon_leader_id_); pm_.neighbor_platoon_info_size_ = platoon_size; - ROS_DEBUG_STREAM("pm_.neighbor_platoon_info_size_: " << pm_.neighbor_platoon_info_size_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.neighbor_platoon_info_size_: " << pm_.neighbor_platoon_info_size_); } } else { - ROS_DEBUG_STREAM("Invalid Mob Op received"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Invalid Mob Op received"); } // Perform state-specific additional actions if (pm_.current_platoon_state == PlatoonState::LEADER) { - mob_op_cb_leader(msg); + mob_op_cb_leader(*msg); } else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) { - mob_op_cb_follower(msg); + mob_op_cb_follower(*msg); } else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) { - mob_op_cb_candidatefollower(msg); + mob_op_cb_candidatefollower(*msg); } else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) { - mob_op_cb_leaderwaiting(msg); + mob_op_cb_leaderwaiting(*msg); } else if (pm_.current_platoon_state == PlatoonState::STANDBY) { - mob_op_cb_standby(msg); + mob_op_cb_standby(*msg); } // UCLA: add leader aborting else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) { - mob_op_cb_leaderaborting(msg); + mob_op_cb_leaderaborting(*msg); } // UCLA: add candidate leader else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) { - mob_op_cb_candidateleader(msg); + mob_op_cb_candidateleader(*msg); } // UCLA: add lead with operation for cut-in join else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) { - mob_op_cb_leadwithoperation(msg); + mob_op_cb_leadwithoperation(*msg); } // UCLA: add prepare to join for cut-in join else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) { - mob_op_cb_preparetojoin(msg); + mob_op_cb_preparetojoin(*msg); } // TODO: Place holder for prepare to depart @@ -927,37 +923,37 @@ namespace platoon_strategic_ihp // INFO messages always processed, STATUS messages if saved in que } - void PlatoonStrategicIHPPlugin::mob_op_cb_standby(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_standby(const carma_v2x_msgs::msg::MobilityOperation& msg) { // In standby state, it will ignore operation message since it is not actively operating - ROS_DEBUG_STREAM("STANDBY state no further action on message from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "STANDBY state no further action on message from " << msg.m_header.sender_id); } // Handle STATUS operation message - void PlatoonStrategicIHPPlugin::mob_op_cb_candidatefollower(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityOperation& msg) { - ROS_DEBUG_STREAM("CANDIDATEFOLLOWER state no further action on message from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CANDIDATEFOLLOWER state no further action on message from " << msg.m_header.sender_id); } // Handle STATUS operation message - void PlatoonStrategicIHPPlugin::mob_op_cb_follower(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_follower(const carma_v2x_msgs::msg::MobilityOperation& msg) { //std::string strategyParams = msg.strategy_params; //bool isPlatoonStatusMsg = (strategyParams.rfind(OPERATION_STATUS_TYPE, 0) == 0); // TODO: Place holder for prepare to depart - ROS_DEBUG_STREAM("FOLLOWER state no further action on message from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "FOLLOWER state no further action on message from " << msg.m_header.sender_id); } // Handle STATUS operation message - void PlatoonStrategicIHPPlugin::mob_op_cb_leaderwaiting(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityOperation& msg) { - ROS_DEBUG_STREAM("LEADERWAITING state no further action on message from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADERWAITING state no further action on message from " << msg.m_header.sender_id); } // UCLA: Handle both STATUS and INFO operation message. Front join and rear join are all handled if incoming operation message have INFO param. - void PlatoonStrategicIHPPlugin::mob_op_cb_leader(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_leader(const carma_v2x_msgs::msg::MobilityOperation& msg) { /** * Note: This is the function to handle the mobility operation message. Vehicle in leader state is either a single ADS vehicle or a platoon leader. @@ -984,7 +980,7 @@ namespace platoon_strategic_ihp bool isPlatoonInfoMsg = strategyParams.rfind(OPERATION_INFO_TYPE, 0) == 0; // INFO message only broadcast by leader and single CAV. bool isInNegotiation = pm_.current_plan.valid || pm_.currentPlatoonID.compare(pm_.dummyID) != 0; // In negotiation indicates host is not available to become a joiner // (i.e., not currently in a platoon or trying to join a platoon). - ROS_DEBUG_STREAM("Top of mob_op_cb_leader, isInNegotiation = " << isInNegotiation); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Top of mob_op_cb_leader, isInNegotiation = " << isInNegotiation); // Condition 1. Host vehicle is the single CAV joining the platoon. if (isPlatoonInfoMsg && !isInNegotiation) @@ -995,7 +991,7 @@ namespace platoon_strategic_ihp // step 1. read INFO message from the target platoon leader // read ecef location from strategy params. - cav_msgs::LocationECEF ecef_loc; + carma_v2x_msgs::msg::LocationECEF ecef_loc; ecef_loc = mob_op_find_ecef_from_INFO_params(strategyParams); // use ecef_loc to calculate front Dtd in m. @@ -1005,7 +1001,7 @@ namespace platoon_strategic_ihp // use ecef_loc to calculate front Ctd in m. double frontVehicleCtd = wm_->routeTrackPos(incoming_pose).crosstrack; // downtrack and crosstrack of the platoon leader --> used for frontal join - ROS_DEBUG_STREAM("Neighbor platoon frontVehicleDtd from ecef: " << frontVehicleDtd << ", frontVehicleCtd from ecef: " << frontVehicleCtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon frontVehicleDtd from ecef: " << frontVehicleDtd << ", frontVehicleCtd from ecef: " << frontVehicleCtd); // use INFO param to find platoon rear vehicle DTD and CTD. double platoon_length = mob_op_find_platoon_length_from_INFO_params(strategyParams); // length of the entire platoon in meters. @@ -1020,16 +1016,16 @@ namespace platoon_strategic_ihp */ double rearVehicleDtd = frontVehicleDtd - platoon_length; - ROS_DEBUG_STREAM("rear veh dtd from platoon length: " << rearVehicleDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rear veh dtd from platoon length: " << rearVehicleDtd); if (!pm_.neighbor_platoon_.empty()) { rearVehicleDtd = pm_.neighbor_platoon_.back().vehiclePosition; - ROS_DEBUG_STREAM("rear veh dtd from neighbor platoon: " << rearVehicleDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rear veh dtd from neighbor platoon: " << rearVehicleDtd); } // Note: For one platoon, we assume all members are in the same lane. double rearVehicleCtd = frontVehicleCtd; - ROS_DEBUG_STREAM("Neighbor platoon rearVehicleDtd: " << rearVehicleDtd << ", rearVehicleCtd: " << rearVehicleCtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon rearVehicleDtd: " << rearVehicleDtd << ", rearVehicleCtd: " << rearVehicleCtd); // Parse the strategy params std::vector inputsParams; @@ -1039,18 +1035,18 @@ namespace platoon_strategic_ihp std::vector targetPlatoonSize_parsed; boost::algorithm::split(targetPlatoonSize_parsed, inputsParams[2], boost::is_any_of(":")); int targetPlatoonSize = std::stoi(targetPlatoonSize_parsed[1]); - ROS_DEBUG_STREAM("target Platoon Size: " << targetPlatoonSize); - ROS_DEBUG_STREAM("Found a vehicle/platoon with id = " << platoonId << " within range."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target Platoon Size: " << targetPlatoonSize); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Found a vehicle/platoon with id = " << platoonId << " within range."); //TODO future: add logic here to assess closeness of the neighbor platoon, as well as its speed, destination // & other attributes to decide if we want to join before assembling a join request // step 2. Generate default info for join request - cav_msgs::MobilityRequest request; + carma_v2x_msgs::msg::MobilityRequest request; request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); request.m_header.recipient_id = senderId; request.m_header.sender_id = config_.vehicleID; - request.m_header.timestamp = ros::Time::now().toNSec()/1000000; + request.m_header.timestamp = timer_factory_->now().nanoseconds()/1000000; request.location = pose_to_ecef(pose_msg_); request.strategy = PLATOONING_STRATEGY; request.urgency = 50; @@ -1063,9 +1059,9 @@ namespace platoon_strategic_ihp /** * Note: "isVehicleRightInFront" tests for same lane */ - ROS_DEBUG_STREAM("Neighbor platoon is right in front of us"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon is right in front of us"); - request.plan_type.type = cav_msgs::PlanType::JOIN_PLATOON_AT_REAR; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_AT_REAR; /* * JOIN_PARAMS format: @@ -1083,7 +1079,7 @@ namespace platoon_strategic_ihp fmter %dummy_join_index; // index = 5 request.strategy_params = fmter.str(); mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Publishing request to leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Publishing request to leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); // Create a new join plan pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); @@ -1092,7 +1088,7 @@ namespace platoon_strategic_ihp if (platoonId.compare(pm_.dummyID) != 0) { pm_.targetPlatoonID = platoonId; - ROS_DEBUG_STREAM("Detected neighbor as a real platoon & storing its ID: " << platoonId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Detected neighbor as a real platoon & storing its ID: " << platoonId); } } @@ -1102,10 +1098,10 @@ namespace platoon_strategic_ihp /** * Note: "isVehicleRightBehind" tests for same lane */ - ROS_DEBUG_STREAM("Neighbor platoon leader is right behind us"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor platoon leader is right behind us"); // UCLA: assign a new plan type - request.plan_type.type = cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT; /** * JOIN_PARAMS format: @@ -1122,7 +1118,7 @@ namespace platoon_strategic_ihp fmter %dummy_join_index; // index = 5 request.strategy_params = fmter.str(); mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Publishing front join request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Publishing front join request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); // Create a new join plan pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); @@ -1132,12 +1128,12 @@ namespace platoon_strategic_ihp if (config_.test_front_join) { pm_.targetPlatoonID = request.m_header.plan_id; - ROS_DEBUG_STREAM("Since neighbor is a fake platoon, storing " << pm_.targetPlatoonID << " as its platoon ID"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Since neighbor is a fake platoon, storing " << pm_.targetPlatoonID << " as its platoon ID"); } else { pm_.targetPlatoonID = platoonId; - ROS_DEBUG_STREAM("Storing real neighbor platoon's ID as target: " << pm_.targetPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Storing real neighbor platoon's ID as target: " << pm_.targetPlatoonID); } } @@ -1146,15 +1142,15 @@ namespace platoon_strategic_ihp && isVehicleNearTargetPlatoon(rearVehicleDtd, frontVehicleDtd, frontVehicleCtd)) { - ROS_DEBUG_STREAM("starting cut-in join process"); - ROS_DEBUG_STREAM("rearVehicleDtd " << rearVehicleDtd); - ROS_DEBUG_STREAM("rearVehicleCtd " << rearVehicleCtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "starting cut-in join process"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rearVehicleDtd " << rearVehicleDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rearVehicleCtd " << rearVehicleCtd); // If we are asking to join an actual platoon (not a solo vehicle), then save its ID for later use if (platoonId.compare(pm_.dummyID) != 0) { pm_.targetPlatoonID = platoonId; - ROS_DEBUG_STREAM("Detected neighbor as a real platoon & storing its ID: " << platoonId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Detected neighbor as a real platoon & storing its ID: " << platoonId); } @@ -1168,17 +1164,17 @@ namespace platoon_strategic_ihp if (!target_lanelets.empty()) { long target_rear_pose_lanelet_id = target_lanelets[0].second.id(); - ROS_DEBUG_STREAM("target_rear_pose_lanelet_id: " << target_rear_pose_lanelet_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_rear_pose_lanelet_id: " << target_rear_pose_lanelet_id); } else { - ROS_DEBUG_STREAM("target_rear_pose_lanelet not found!!"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_rear_pose_lanelet not found!!"); } } else { - ROS_DEBUG_STREAM("No target pose is found, so we cannot prodeed with a cutin join request."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "No target pose is found, so we cannot prodeed with a cutin join request."); return; } @@ -1198,7 +1194,7 @@ namespace platoon_strategic_ihp // Note: Request conposed outside of if conditions // UCLA: Desired joining index for cut-in join, indicate the index of gap-leading vehicle. -1 indicate cut-in from front. // Note: remove join_index to info param. - request.plan_type.type = cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN; // At this step all cut-in types start with this request, so the join_index at this point is set to default, -2. int join_index = -2; @@ -1211,7 +1207,7 @@ namespace platoon_strategic_ihp fmter %join_index; // index = 5 request.strategy_params = fmter.str(); mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Publishing request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Publishing request to the leader " << senderId << " with params " << request.strategy_params << " and plan id = " << request.m_header.plan_id); // Create a new join plan pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); @@ -1220,7 +1216,7 @@ namespace platoon_strategic_ihp // step 6. Return none if no platoon nearby else { - ROS_DEBUG_STREAM("Ignore platoon with platoon id: " << platoonId << " because it is too far away to join."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore platoon with platoon id: " << platoonId << " because it is too far away to join."); } } @@ -1229,25 +1225,25 @@ namespace platoon_strategic_ihp } // UCLA: mob_op_cb for the new leader aborting state (inherited from candidate follower), handle STATUS message. - void PlatoonStrategicIHPPlugin::mob_op_cb_leaderaborting(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityOperation& msg) { - ROS_DEBUG_STREAM("LEADERABORTING state no further action on message from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADERABORTING state no further action on message from " << msg.m_header.sender_id); } // UCLA: mob_op_candidateleader for the new candidate leader state (inherited from leader waiting), handle STATUS message. - void PlatoonStrategicIHPPlugin::mob_op_cb_candidateleader(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_candidateleader(const carma_v2x_msgs::msg::MobilityOperation& msg) { - ROS_DEBUG_STREAM("CANDIDATELEADER state no further action on message from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CANDIDATELEADER state no further action on message from " << msg.m_header.sender_id); } // UCLA: Mobility operation callback for lead_with_operation state (cut-in join). - void PlatoonStrategicIHPPlugin::mob_op_cb_leadwithoperation(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityOperation& msg) { - ROS_DEBUG_STREAM("LEADWITHOPERATION state no further action on message from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADWITHOPERATION state no further action on message from " << msg.m_header.sender_id); } // UCLA: Mobility operation callback for prepare to join state (cut-in join). - void PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin(const cav_msgs::MobilityOperation& msg) + void PlatoonStrategicIHPPlugin::mob_op_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityOperation& msg) { /* * If same lane with leader, then send request to do same lane join. @@ -1264,7 +1260,7 @@ namespace platoon_strategic_ihp // If this is an INFO message and our record of the neighbor platoon is complete then // pm_.is_neighbor_record_complete_ = true; //TODO temporary - ROS_DEBUG_STREAM("pm_.is_neighbor_record_complete_" << pm_.is_neighbor_record_complete_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.is_neighbor_record_complete_" << pm_.is_neighbor_record_complete_); if (isPlatoonInfoMsg && pm_.is_neighbor_record_complete_) { @@ -1274,7 +1270,7 @@ namespace platoon_strategic_ihp // aborted. // read ecef location from strategy params. - cav_msgs::LocationECEF ecef_loc; + carma_v2x_msgs::msg::LocationECEF ecef_loc; ecef_loc = mob_op_find_ecef_from_INFO_params(strategyParams); // use ecef_loc to calculate front Dtd in m. lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(ecef_loc); @@ -1286,9 +1282,9 @@ namespace platoon_strategic_ihp // // Find neighbor platoon end vehicle and its downtrack in m int rearVehicleIndex = pm_.neighbor_platoon_.size() - 1; - ROS_DEBUG_STREAM("rearVehicleIndex: " << rearVehicleIndex); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "rearVehicleIndex: " << rearVehicleIndex); double rearVehicleDtd = pm_.neighbor_platoon_[rearVehicleIndex].vehiclePosition; - ROS_DEBUG_STREAM("Neighbor rearVehicleDtd from ecef: " << rearVehicleDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Neighbor rearVehicleDtd from ecef: " << rearVehicleDtd); // If lane change has not yet been authorized, stop here (this method will be running before the negotiations // with the platoon leader are complete) @@ -1299,29 +1295,29 @@ namespace platoon_strategic_ihp // determine if the lane change is finished bool isSameLaneWithPlatoon = abs(frontVehicleCtd - current_crosstrack_) <= config_.maxCrosstrackError; - ROS_DEBUG_STREAM("Lane change has been authorized. isSameLaneWithPlatoon = " << isSameLaneWithPlatoon); - ROS_DEBUG_STREAM("crosstrack diff" << abs(frontVehicleCtd - current_crosstrack_)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane change has been authorized. isSameLaneWithPlatoon = " << isSameLaneWithPlatoon); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "crosstrack diff" << abs(frontVehicleCtd - current_crosstrack_)); if (isSameLaneWithPlatoon) { // request 1. reset the safeToChangLane indicators if lane change is finished safeToLaneChange_ = false; // request 2. change to same lane operation states (determine based on DTD differences) - cav_msgs::MobilityRequest request; + carma_v2x_msgs::msg::MobilityRequest request; request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); request.m_header.recipient_id = senderId; request.m_header.sender_id = config_.vehicleID; - request.m_header.timestamp = ros::Time::now().toNSec()/1000000; + request.m_header.timestamp = timer_factory_->now().nanoseconds()/1000000; request.location = pose_to_ecef(pose_msg_); // UCLA: send request based on cut-in type if (frontVehicleDtd < current_downtrack_) { - request.plan_type.type = cav_msgs::PlanType::CUT_IN_FRONT_DONE; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE; } else { - request.plan_type.type = cav_msgs::PlanType::CUT_IN_MID_OR_REAR_DONE; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::CUT_IN_MID_OR_REAR_DONE; } request.strategy = PLATOONING_STRATEGY; double host_platoon_size = pm_.getHostPlatoonSize(); @@ -1342,14 +1338,14 @@ namespace platoon_strategic_ihp pm_.currentPlatoonID = request.m_header.plan_id; } - ROS_DEBUG_STREAM("new platoon id: " << pm_.currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "new platoon id: " << pm_.currentPlatoonID); pm_.current_plan = ActionPlan(true, request.m_header.timestamp, request.m_header.plan_id, senderId); - ROS_DEBUG_STREAM("Published Mobility request to revert to same-lane operation"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility request to revert to same-lane operation"); } else { - ROS_DEBUG_STREAM("Lane Change not completed"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane Change not completed"); } } } @@ -1357,7 +1353,7 @@ namespace platoon_strategic_ihp // TODO: Place holder for prepare to depart (mob_op_cb_depart) //------- 3. Mobility Request Callback ------- - MobilityRequestResponse PlatoonStrategicIHPPlugin::handle_mob_req(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::handle_mob_req(const carma_v2x_msgs::msg::MobilityRequest& msg) { MobilityRequestResponse mobility_response = MobilityRequestResponse::NO_RESPONSE; @@ -1365,7 +1361,7 @@ namespace platoon_strategic_ihp std::string strategy = msg.strategy; if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0) { - ROS_DEBUG_STREAM("Ignoring mobility operation message for " << strategy << " strategy."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring mobility operation message for " << strategy << " strategy."); return MobilityRequestResponse::NO_RESPONSE; } @@ -1416,22 +1412,22 @@ namespace platoon_strategic_ihp return mobility_response; } - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_standby(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_standby(const carma_v2x_msgs::msg::MobilityRequest& msg) { // In standby state, the plugin is not responsible for replying to any request messages - ROS_DEBUG_STREAM("STANDBY state does nothing with msg from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "STANDBY state does nothing with msg from " << msg.m_header.sender_id); return MobilityRequestResponse::NO_RESPONSE; } - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_candidatefollower(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityRequest& msg) { // This state does not handle any mobility request for now // TODO Maybe it should handle some ABORT request from a waiting leader - ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " but ignored."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " but ignored."); return MobilityRequestResponse::NO_RESPONSE; } - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_follower(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_follower(const carma_v2x_msgs::msg::MobilityRequest& msg) { /** * For cut-in join, the gap rear vehicle need to slow down once they received the request from platoon leader. @@ -1443,13 +1439,13 @@ namespace platoon_strategic_ihp * and switch to candidate follower state. While the previous leader depart and operating in single leader state. */ - cav_msgs::PlanType plan_type = msg.plan_type; + carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type; std::string reccipientID = msg.m_header.recipient_id; std::string reqSenderID = msg.m_header.sender_id; // Check joining plan type. - bool isCutInJoin = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; - bool isGapCreated = plan_type.type == cav_msgs::PlanType::STOP_CREATE_GAP; + bool isCutInJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN; + bool isGapCreated = plan_type.type == carma_v2x_msgs::msg::PlanType::STOP_CREATE_GAP; // TODO: Place holder for departure // Check if host is intended recipient @@ -1464,15 +1460,15 @@ namespace platoon_strategic_ihp std::vector join_index_parsed; boost::algorithm::split(join_index_parsed, inputsParams[5], boost::is_any_of(":")); int req_sender_join_index = std::stoi(join_index_parsed[1]); - ROS_DEBUG_STREAM("Requesting join_index parsed: " << req_sender_join_index); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Requesting join_index parsed: " << req_sender_join_index); // Control vehicle speed based on cut-in type // 1. cut-in from rear if (static_cast(req_sender_join_index) == pm_.host_platoon_.size()-1) { // Accept plan and idle (becasue rear join, gap leading vehicle do not need to slow down). - ROS_WARN("Requested cut-in from rear, start approve cut-in and wait for lane change."); - ROS_WARN("Due to the rear join nature, there is no need to slow down or create gap."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Requested cut-in from rear, start approve cut-in and wait for lane change."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Due to the rear join nature, there is no need to slow down or create gap."); return MobilityRequestResponse::ACK; } @@ -1481,14 +1477,14 @@ namespace platoon_strategic_ihp { // Accept plan and slow down to create gap. pm_.isCreateGap = true; - ROS_DEBUG_STREAM("Requested cut-in index is: " << req_sender_join_index << ", approve cut-in and start create gap."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Requested cut-in index is: " << req_sender_join_index << ", approve cut-in and start create gap."); return MobilityRequestResponse::ACK; } // 3. Abnormal join index else { // Note: Leader will abort plan if reponse is not ACK for plantype "PLATOON_CUT_IN_JOIN". - ROS_DEBUG_STREAM("Abnormal cut-in index, abort operation."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Abnormal cut-in index, abort operation."); return MobilityRequestResponse::NACK; } } @@ -1497,7 +1493,7 @@ namespace platoon_strategic_ihp // 4. Reset to normal speed once the gap is created. else if (isGapCreated) { - ROS_DEBUG_STREAM("Gap is created, revert to normal operating speed."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Gap is created, revert to normal operating speed."); // Only reset create gap indicator, no need to send response. pm_.isCreateGap = false; return MobilityRequestResponse::NO_RESPONSE; @@ -1513,16 +1509,16 @@ namespace platoon_strategic_ihp } // Middle state that decided whether to accept joiner - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leaderwaiting(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityRequest& msg) { bool isTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; - bool isCandidateJoin = msg.plan_type.type == cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; + bool isCandidateJoin = msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_FOLLOWER_JOIN; lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); double obj_cross_track = wm_->routeTrackPos(incoming_pose).crosstrack; bool inTheSameLane = abs(obj_cross_track - current_crosstrack_) < config_.maxCrosstrackError; - ROS_DEBUG_STREAM("current_cross_track error = " << abs(obj_cross_track - current_crosstrack_)); - ROS_DEBUG_STREAM("inTheSameLane = " << inTheSameLane); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_cross_track error = " << abs(obj_cross_track - current_crosstrack_)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "inTheSameLane = " << inTheSameLane); // If everything is agreeable then approve the request; if it is from an unexpected vehicle or // is not a candidate join request, then we can just ignore it with no action @@ -1531,8 +1527,8 @@ namespace platoon_strategic_ihp { if (inTheSameLane) { - ROS_DEBUG_STREAM("Target vehicle " << pm_.current_plan.peerId << " is actually joining."); - ROS_DEBUG_STREAM("Changing to PlatoonLeaderState and send ACK to target vehicle"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Target vehicle " << pm_.current_plan.peerId << " is actually joining."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Changing to PlatoonLeaderState and send ACK to target vehicle"); // Change state to LEADER pm_.current_platoon_state = PlatoonState::LEADER; @@ -1547,9 +1543,9 @@ namespace platoon_strategic_ihp PlatoonMember newMember = PlatoonMember(); newMember.staticId = msg.m_header.sender_id; newMember.vehiclePosition = wm_->routeTrackPos(incoming_pose).downtrack; - ROS_DEBUG_STREAM("New member being added to platoon vector whose size is currently " << pm_.host_platoon_.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "New member being added to platoon vector whose size is currently " << pm_.host_platoon_.size()); pm_.host_platoon_.push_back(newMember); - ROS_DEBUG_STREAM("pm_ now thinks platoon size is " << pm_.getHostPlatoonSize()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_ now thinks platoon size is " << pm_.getHostPlatoonSize()); // Send approval of the request response = MobilityRequestResponse::ACK; @@ -1558,13 +1554,13 @@ namespace platoon_strategic_ihp } else //correct vehicle and intent, but it's in the wrong lane { - ROS_DEBUG_STREAM("Received platoon request with vehicle id = " << msg.m_header.sender_id << " but in wrong lane. NACK"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received platoon request with vehicle id = " << msg.m_header.sender_id << " but in wrong lane. NACK"); response = MobilityRequestResponse::NACK; // // Remove the candidate joiner from the platoon structure // if (!pm_.removeMemberById(msg.m_header.sender_id)) // { - // ROS_DEBUG_STREAM("Failed to remove candidate joiner from platoon record: " << msg.m_header.sender_id); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Failed to remove candidate joiner from platoon record: " << msg.m_header.sender_id); // } } } @@ -1574,7 +1570,7 @@ namespace platoon_strategic_ihp } // UCLA: add condition to handle frontal join request - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leader(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leader(const carma_v2x_msgs::msg::MobilityRequest& msg) { /** * UCLA implementation note: @@ -1589,29 +1585,29 @@ namespace platoon_strategic_ihp */ // Check joining plan type. - cav_msgs::PlanType plan_type = msg.plan_type; + carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type; /** * Note: * JOIN_FROM_FRONT indicate a same-lane front join. * JOIN_PLATOON_AT_REAR indicate a same-lane rear join. * PLATOON_CUT_IN_JOIN indicate a cut-in join, which include three cut-in methods: cut-in front, cut-in middle, and cut-in rear. */ - bool isFrontJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT; - bool isRearJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_AT_REAR; - bool isCutInJoin = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; - bool isDepart = (plan_type.type == cav_msgs::PlanType::PLATOON_DEPARTURE); + bool isFrontJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT; + bool isRearJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_AT_REAR; + bool isCutInJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN; + bool isDepart = (plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_DEPARTURE); // Ignore the request if we are already working with a join/departure process or if no join type was requested (prevents multiple applicants) if (isFrontJoin || isRearJoin || isCutInJoin || isDepart) { if (pm_.current_plan.valid){ - ROS_DEBUG_STREAM("Ignoring incoming request since we are already negotiating a join."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring incoming request since we are already negotiating a join."); return MobilityRequestResponse::NO_RESPONSE; //TODO: replace with NACK that indicates to ask me later } } else { - ROS_WARN_STREAM("Received request with bogus message type " << plan_type.type << "; ignoring"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received request with bogus message type " << plan_type.type << "; ignoring"); return MobilityRequestResponse::NO_RESPONSE; } @@ -1620,14 +1616,14 @@ namespace platoon_strategic_ihp // We are currently checking two basic JOIN conditions: // 1. The size limitation on current platoon based on the plugin's parameters. // 2. Calculate how long that vehicle can be in a reasonable distance to actually join us. - cav_msgs::MobilityHeader msgHeader = msg.m_header; + carma_v2x_msgs::msg::MobilityHeader msgHeader = msg.m_header; std::string params = msg.strategy_params; std::string applicantId = msgHeader.sender_id; - ROS_DEBUG_STREAM("Received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id); - ROS_DEBUG_STREAM("The strategy parameters are " << params); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The strategy parameters are " << params); if (params.length() == 0) { - ROS_DEBUG_STREAM("The strategy parameters are empty, return no response"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The strategy parameters are empty, return no response"); return MobilityRequestResponse::NO_RESPONSE; } @@ -1639,24 +1635,24 @@ namespace platoon_strategic_ihp std::vector applicantSize_parsed; boost::algorithm::split(applicantSize_parsed, inputsParams[0], boost::is_any_of(":")); int applicantSize = std::stoi(applicantSize_parsed[1]); - ROS_DEBUG_STREAM("applicantSize: " << applicantSize); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantSize: " << applicantSize); // Parse applicant Current Speed in m/s std::vector applicantCurrentSpeed_parsed; boost::algorithm::split(applicantCurrentSpeed_parsed, inputsParams[1], boost::is_any_of(":")); double applicantCurrentSpeed = std::stod(applicantCurrentSpeed_parsed[1]); - ROS_DEBUG_STREAM("applicantCurrentSpeed: " << applicantCurrentSpeed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantCurrentSpeed: " << applicantCurrentSpeed); // Calculate downtrack (m) based on incoming pose. lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); double applicantCurrentDtd = wm_->routeTrackPos(incoming_pose).downtrack; - ROS_DEBUG_STREAM("applicantCurrentmemberUpdates from ecef pose: " << applicantCurrentDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantCurrentmemberUpdates from ecef pose: " << applicantCurrentDtd); // Calculate crosstrack (m) based on incoming pose. double applicantCurrentCtd = wm_->routeTrackPos(incoming_pose).crosstrack; - ROS_DEBUG_STREAM("applicantCurrentCtd from ecef pose: " << applicantCurrentCtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "applicantCurrentCtd from ecef pose: " << applicantCurrentCtd); bool isInLane = abs(applicantCurrentCtd - current_crosstrack_) < config_.maxCrosstrackError; - ROS_DEBUG_STREAM("isInLane = " << isInLane); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isInLane = " << isInLane); // Check if we have enough room for that applicant int currentPlatoonSize = pm_.getHostPlatoonSize(); @@ -1666,20 +1662,20 @@ namespace platoon_strategic_ihp if (isRearJoin) { // Log the request type - ROS_DEBUG_STREAM("The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane REAR-JOIN request !"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane REAR-JOIN request !"); // -- core condition to decided accept joiner or not if (hasEnoughRoomInPlatoon && isInLane) { - ROS_DEBUG_STREAM("The current platoon has enough room for the applicant with size " << applicantSize); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon has enough room for the applicant with size " << applicantSize); double currentRearDtd = pm_.getPlatoonRearDowntrackDistance(); - ROS_DEBUG_STREAM("The current platoon rear dtd is " << currentRearDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon rear dtd is " << currentRearDtd); double currentGap = currentRearDtd - applicantCurrentDtd - config_.vehicleLength; double currentTimeGap = currentGap / applicantCurrentSpeed; - ROS_DEBUG_STREAM("The gap between current platoon rear and applicant is " << currentGap << "m or " << currentTimeGap << "s"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The gap between current platoon rear and applicant is " << currentGap << "m or " << currentTimeGap << "s"); if (currentGap < config_.minAllowedJoinGap) { - ROS_WARN("We should not receive any request from the vehicle in front of us. NACK it."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"We should not receive any request from the vehicle in front of us. NACK it."); return MobilityRequestResponse::NACK; } @@ -1687,25 +1683,25 @@ namespace platoon_strategic_ihp bool isDistanceCloseEnough = currentGap <= config_.maxAllowedJoinGap || currentTimeGap <= config_.maxAllowedJoinTimeGap; if (isDistanceCloseEnough) { - ROS_DEBUG_STREAM("The applicant is close enough and we will allow it to try to join"); - ROS_DEBUG_STREAM("Change to LeaderWaitingState and waiting for " << msg.m_header.sender_id << " to join"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is close enough and we will allow it to try to join"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to LeaderWaitingState and waiting for " << msg.m_header.sender_id << " to join"); // change state to leaderwaiting ! pm_.current_platoon_state = PlatoonState::LEADERWAITING; - waitingStartTime = ros::Time::now().toNSec() / 1000000; + waitingStartTime = timer_factory_->now().nanoseconds() / 1000000; pm_.current_plan = ActionPlan(true, waitingStartTime, msgHeader.plan_id, applicantId); pm_.platoonLeaderID = pm_.HostMobilityId; return MobilityRequestResponse::ACK; } else { - ROS_DEBUG_STREAM("The applicant is too far away from us. NACK."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is too far away from us. NACK."); return MobilityRequestResponse::NACK; //TODO: add reason & request to try again } } else { - ROS_DEBUG_STREAM("The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK"); return MobilityRequestResponse::NACK; } } @@ -1714,24 +1710,24 @@ namespace platoon_strategic_ihp else if (isFrontJoin) { // Log the request type - ROS_DEBUG_STREAM("The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane FRONT-JOIN request !"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a same-lane FRONT-JOIN request !"); // -- core condition to decided accept joiner or not if (hasEnoughRoomInPlatoon && isInLane) { - ROS_DEBUG_STREAM("The current platoon has enough room for the applicant with size " << applicantSize); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon has enough room for the applicant with size " << applicantSize); // UCLA: change to read platoon front info double currentFrontDtd = pm_.getPlatoonFrontDowntrackDistance(); - ROS_DEBUG_STREAM("The current platoon front dtd is " << currentFrontDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon front dtd is " << currentFrontDtd); // UCLA: adjust for calculating gap between new leader and old leader double currentGap = applicantCurrentDtd - currentFrontDtd - config_.vehicleLength; double currentTimeGap = currentGap / applicantCurrentSpeed; - ROS_DEBUG_STREAM("The gap between current platoon front and applicant is " << currentGap << "m or " << currentTimeGap << "s"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The gap between current platoon front and applicant is " << currentGap << "m or " << currentTimeGap << "s"); if (currentGap < config_.minAllowedJoinGap) { - ROS_WARN("The current time gap is not suitable for frontal join. NACK it."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"The current time gap is not suitable for frontal join. NACK it."); return MobilityRequestResponse::NACK; } @@ -1743,15 +1739,15 @@ namespace platoon_strategic_ihp if (isDistanceCloseEnough && isPlatoonNotSingle) { - ROS_DEBUG_STREAM("The applicant is close enough for frontal join, send acceptance response"); - ROS_DEBUG_STREAM("Change to LeaderAborting state and waiting for " << msg.m_header.sender_id << " to join as the new platoon leader"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is close enough for frontal join, send acceptance response"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to LeaderAborting state and waiting for " << msg.m_header.sender_id << " to join as the new platoon leader"); // ----------------- give up leader position and look for new leader -------------------------- // adjust for frontal join. Platoon info is related to the platoon at back of the candidate leader vehicle. // Don't want an action plan here pm_.current_platoon_state = PlatoonState::LEADERABORTING; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; // If we are testing with a single vehicle representing this platoon, then we don't yet have a platoon ID, // so use the ID for the proposed joining action plan @@ -1763,19 +1759,19 @@ namespace platoon_strategic_ihp // Store the leader ID as that of the joiner to allow run_leader_aborting to work correctly pm_.platoonLeaderID = applicantId; - waitingStartTime = ros::Time::now().toNSec() / 1000000; + waitingStartTime = timer_factory_->now().nanoseconds() / 1000000; pm_.current_plan.valid = false; return MobilityRequestResponse::ACK; } else { - ROS_DEBUG_STREAM("The joining gap (" << currentGap << " m) is too far away from us or the target platoon size (" << pm_.getHostPlatoonSize() << ") is one. NACK."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining gap (" << currentGap << " m) is too far away from us or the target platoon size (" << pm_.getHostPlatoonSize() << ") is one. NACK."); return MobilityRequestResponse::NACK; //TODO: add reason & request to try again } } else { - ROS_DEBUG_STREAM("The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon does not have enough room for applicant of size " << applicantSize << ". NACK"); return MobilityRequestResponse::NACK; } } @@ -1784,28 +1780,28 @@ namespace platoon_strategic_ihp else if (isCutInJoin) { // Log the request type - ROS_DEBUG_STREAM("The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a CUT-IN-JOIN request !"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The received mobility JOIN request from " << applicantId << " and PlanId = " << msgHeader.plan_id << " is a CUT-IN-JOIN request !"); // -- core condition to decided accept joiner or not. It is necessary leader only process the first cut-in joining request. // Note: The host is the platoon leader, need to use a different method to determine if joining vehicle is nearby. if (hasEnoughRoomInPlatoon && isJoiningVehicleNearPlatoon(applicantCurrentDtd, applicantCurrentCtd)) { - ROS_DEBUG_STREAM("The current platoon has enough room for the applicant with size " << applicantSize); - ROS_DEBUG_STREAM("The applicant is close enough for cut-in join, send acceptance response"); - ROS_DEBUG_STREAM("Change to Leading with operation state and waiting for " << msg.m_header.sender_id << " to change lane"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon has enough room for the applicant with size " << applicantSize); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant is close enough for cut-in join, send acceptance response"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to Leading with operation state and waiting for " << msg.m_header.sender_id << " to change lane"); // change state to lead with operation pm_.current_platoon_state = PlatoonState::LEADWITHOPERATION; - waitingStartTime = ros::Time::now().toNSec() / 1000000; + waitingStartTime = timer_factory_->now().nanoseconds() / 1000000; pm_.current_plan = ActionPlan(true, waitingStartTime, msgHeader.plan_id, applicantId); pm_.platoonLeaderID = pm_.HostMobilityId; return MobilityRequestResponse::ACK; } else { - ROS_DEBUG_STREAM("The current platoon does not have enough room or the applicant is too far away from us. NACK the request."); - ROS_DEBUG_STREAM("The current applicant size: " << applicantSize << "."); - ROS_DEBUG_STREAM("The applicant downtrack is: " << current_downtrack_ << "."); - ROS_DEBUG_STREAM("The applicant crosstrack is: " << current_crosstrack_ << "."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current platoon does not have enough room or the applicant is too far away from us. NACK the request."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current applicant size: " << applicantSize << "."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant downtrack is: " << current_downtrack_ << "."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The applicant crosstrack is: " << current_crosstrack_ << "."); return MobilityRequestResponse::NACK; } } @@ -1815,39 +1811,39 @@ namespace platoon_strategic_ihp // no response else { - ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " and ignored."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " and ignored."); return MobilityRequestResponse::NO_RESPONSE; } } // UCLA: mobility request leader aborting (inherited from candidate follower) - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leaderaborting(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityRequest& msg) { // This state does not handle any mobility request for now // TODO Maybe it should handle some ABORT request from a candidate leader - ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " but ignored."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " but ignored."); return MobilityRequestResponse::NO_RESPONSE; } // UCLA: mobility request candidate leader (inherited from leader waiting) - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_candidateleader(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_candidateleader(const carma_v2x_msgs::msg::MobilityRequest& msg) { bool isTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; // need to check: senderID (old leader) - bool isCandidateJoin = msg.plan_type.type == cav_msgs::PlanType::PLATOON_FRONT_JOIN; + bool isCandidateJoin = msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_FRONT_JOIN; lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); double obj_cross_track = wm_->routeTrackPos(incoming_pose).crosstrack; bool inTheSameLane = abs(obj_cross_track - current_crosstrack_) < config_.maxCrosstrackError; - ROS_DEBUG_STREAM("current_cross_track error = " << abs(obj_cross_track - current_crosstrack_)); - ROS_DEBUG_STREAM("obj_cross_track = " << obj_cross_track); - ROS_DEBUG_STREAM("current_crosstrack_ = " << current_crosstrack_); - ROS_DEBUG_STREAM("inTheSameLane = " << inTheSameLane); - ROS_DEBUG_STREAM("isTargetVehicle = " << isTargetVehicle); - ROS_DEBUG_STREAM("isCandidateJoin = " << isCandidateJoin); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_cross_track error = " << abs(obj_cross_track - current_crosstrack_)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "obj_cross_track = " << obj_cross_track); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_crosstrack_ = " << current_crosstrack_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "inTheSameLane = " << inTheSameLane); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isTargetVehicle = " << isTargetVehicle); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isCandidateJoin = " << isCandidateJoin); if (isCandidateJoin && inTheSameLane && isTargetVehicle) { - ROS_DEBUG_STREAM("Old platoon leader " << pm_.current_plan.peerId << " has agreed to joining."); - ROS_DEBUG_STREAM("Changing to PlatoonLeaderState and send ACK to the previous leader vehicle"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Old platoon leader " << pm_.current_plan.peerId << " has agreed to joining."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Changing to PlatoonLeaderState and send ACK to the previous leader vehicle"); pm_.current_platoon_state = PlatoonState::LEADER; // Clean up planning info @@ -1861,8 +1857,8 @@ namespace platoon_strategic_ihp } else { - ROS_DEBUG_STREAM("Received platoon request with vehicle id = " << msg.m_header.sender_id); - ROS_DEBUG_STREAM("The request type is " << msg.plan_type.type << " and we choose to ignore"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received platoon request with vehicle id = " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The request type is " << msg.plan_type.type << " and we choose to ignore"); pm_.clearActionPlan(); pm_.resetHostPlatoon(); //ASSUMES host is a solo joiner @@ -1873,7 +1869,7 @@ namespace platoon_strategic_ihp } // UCLA: add request call-back function for lead with operation state (for cut-in join) - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityRequest& msg) { /* * Current leader change state to lead with opertaion once the cut-in join request is accepeted. @@ -1885,7 +1881,7 @@ namespace platoon_strategic_ihp */ // Check request plan type - cav_msgs::PlanType plan_type = msg.plan_type; + carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type; std::string strategyParams = msg.strategy_params; std::string reqSenderID = msg.m_header.sender_id; @@ -1893,7 +1889,7 @@ namespace platoon_strategic_ihp lanelet::BasicPoint2d incoming_pose = ecef_to_map_point(msg.location); // read downtrack double applicantCurrentDtd = wm_->routeTrackPos(incoming_pose).downtrack; - ROS_DEBUG_STREAM("Applicant downtrack from ecef pose: " << applicantCurrentDtd); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Applicant downtrack from ecef pose: " << applicantCurrentDtd); // Read requesting join index std::vector inputsParams; @@ -1902,9 +1898,9 @@ namespace platoon_strategic_ihp std::vector join_index_parsed; boost::algorithm::split(join_index_parsed, inputsParams[5], boost::is_any_of(":")); int req_sender_join_index = std::stoi(join_index_parsed[1]); - ROS_DEBUG_STREAM("Requesting join_index parsed: " << req_sender_join_index); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Requesting join_index parsed: " << req_sender_join_index); - if (plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN) + if (plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN) { // Send response // ----- CUT-IN front ----- @@ -1917,21 +1913,21 @@ namespace platoon_strategic_ihp if (isFrontJoinerInPosition) { - ROS_DEBUG_STREAM("The joining vehicle is cutting in from front. Gap is already sufficient."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is cutting in from front. Gap is already sufficient."); return MobilityRequestResponse::ACK; } else if (cutinDtdDifference > 0.0 && cutinDtdDifference < 1.5*config_.vehicleLength) { // slow down leader to allow joiner cut-in pm_.isCreateGap = true; - ROS_DEBUG_STREAM("The joining vehicle is cutting in from front."); - ROS_DEBUG_STREAM("Host (leader) slow down notified, joining vehicle can prepare to join"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is cutting in from front."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host (leader) slow down notified, joining vehicle can prepare to join"); return MobilityRequestResponse::ACK; } else { - ROS_DEBUG_STREAM("Front join geometry violation. NACK. cutinDtdDifference = " << cutinDtdDifference); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Front join geometry violation. NACK. cutinDtdDifference = " << cutinDtdDifference); pm_.current_platoon_state = PlatoonState::LEADER; return MobilityRequestResponse::NACK; } @@ -1949,14 +1945,14 @@ namespace platoon_strategic_ihp if (isRearJoinerInPosition) { - ROS_DEBUG_STREAM("Published Mobility cut-in-rear-Join request to relavent platoon member, host is leader."); - ROS_WARN("Published Mobility cut-in-rear-Join request to relavent platoon members to signal gap creation."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility cut-in-rear-Join request to relavent platoon member, host is leader."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Published Mobility cut-in-rear-Join request to relavent platoon members to signal gap creation."); pm_.isCreateGap = true; return MobilityRequestResponse::ACK; } else { - ROS_DEBUG_STREAM("Rear join geometry violation. NACK. rearGap = " << rearGap); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Rear join geometry violation. NACK. rearGap = " << rearGap); pm_.current_platoon_state = PlatoonState::LEADER; return MobilityRequestResponse::NACK; } @@ -1975,14 +1971,14 @@ namespace platoon_strategic_ihp if (isMidJoinerInPosition) { // compose request (Note: The recipient should be the gap following vehicle.) - cav_msgs::MobilityRequest request; + carma_v2x_msgs::msg::MobilityRequest request; request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); // Note: For cut-in mid, notify gap rear member to create/increase gap. std::string recipient_ID = pm_.host_platoon_[req_sender_join_index+1].staticId; request.m_header.sender_id = config_.vehicleID; - request.m_header.timestamp = ros::Time::now().toNSec() / 1000000;; + request.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;; // UCLA: add plan type, add this in cav_mwgs/plan_type - request.plan_type.type = cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN; request.strategy = PLATOONING_STRATEGY; double platoon_size = pm_.getHostPlatoonSize(); @@ -2001,14 +1997,14 @@ namespace platoon_strategic_ihp // note: for rear join, cut-in index == host_platoon_.size()-1; for join from front, index == -1 // for cut-in in middle, index indicate the gap leading vehicle's index mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation, host is leader."); - ROS_WARN("Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation."); - ROS_DEBUG_STREAM("The joining vehicle is cutting in at index: "<< req_sender_join_index <<". Notify gap rear vehicle with ID: " << recipient_ID << " to slow down"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation, host is leader."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Published Mobility cut-in-mid-Join request to relavent platoon members to signal gap creation."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The joining vehicle is cutting in at index: "<< req_sender_join_index <<". Notify gap rear vehicle with ID: " << recipient_ID << " to slow down"); return MobilityRequestResponse::ACK; } else { - ROS_DEBUG_STREAM("Mid join geometry violation. NACK. gapFollwerDiff = " << gapFollowerDiff); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Mid join geometry violation. NACK. gapFollwerDiff = " << gapFollowerDiff); pm_.current_platoon_state = PlatoonState::LEADER; return MobilityRequestResponse::NACK; } @@ -2016,7 +2012,7 @@ namespace platoon_strategic_ihp } // task 2: For cut-in from front, the leader need to stop creating gap - else if (plan_type.type == cav_msgs::PlanType::STOP_CREATE_GAP) + else if (plan_type.type == carma_v2x_msgs::msg::PlanType::STOP_CREATE_GAP) { // reset create gap indicator pm_.isCreateGap = false; @@ -2025,11 +2021,11 @@ namespace platoon_strategic_ihp } // task 3 cut-in front: After creating gap, revert back to same-lane operation - else if (plan_type.type == cav_msgs::PlanType::CUT_IN_FRONT_DONE) + else if (plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE) { - ROS_DEBUG_STREAM("Cut-in from front lane change finished, leader revert to same-lane maneuver."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from front lane change finished, leader revert to same-lane maneuver."); pm_.current_platoon_state = PlatoonState::LEADERABORTING; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; // if testing with two vehicles, use plan id as platoon id if (pm_.currentPlatoonID.compare(pm_.dummyID) == 0) // if (config_.allowCutinJoin) @@ -2043,11 +2039,11 @@ namespace platoon_strategic_ihp } // task 4 cut-in from middle/rear - else if (plan_type.type == cav_msgs::PlanType::CUT_IN_MID_OR_REAR_DONE) + else if (plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_MID_OR_REAR_DONE) { - ROS_DEBUG_STREAM("Cut-in from mid/rear lane change finished, leader revert to same-lane maneuver."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from mid/rear lane change finished, leader revert to same-lane maneuver."); pm_.current_platoon_state = PlatoonState::LEADERWAITING; - waitingStartTime = ros::Time::now().toNSec() / 1000000; + waitingStartTime = timer_factory_->now().nanoseconds() / 1000000; return MobilityRequestResponse::ACK; } @@ -2055,23 +2051,23 @@ namespace platoon_strategic_ihp // task 5: if other joining vehicle send joning request, NACK it since there is already a cut-in join going on. else { - ROS_DEBUG_STREAM("CUT-IN join maneuver is already in operation, NACK incoming join requests from other candidates."); - ROS_DEBUG_STREAM("Plan Type: " << plan_type.type ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CUT-IN join maneuver is already in operation, NACK incoming join requests from other candidates."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Plan Type: " << plan_type.type ); return MobilityRequestResponse::NACK; } // this statement should never be reached, but will ensure reasonable behavior in case of coding error above - ROS_WARN_STREAM("End of method reached! Apparent logic fault above."); - ROS_DEBUG_STREAM("End of method reached! Apparent logic fault above."); //since WARN doesn't always print + RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "End of method reached! Apparent logic fault above."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "End of method reached! Apparent logic fault above."); //since WARN doesn't always print return MobilityRequestResponse::NO_RESPONSE; } // UCLA: add request call-back function for prepare to join (for cut-in join) - MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_preparetojoin(const cav_msgs::MobilityRequest& msg) + MobilityRequestResponse PlatoonStrategicIHPPlugin::mob_req_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityRequest& msg) { // This state does not handle any mobility request for now // TODO: if joining vehicle need to adjust speed, the leader should request it and the request should be handled here. - ROS_DEBUG_STREAM("Received mobility request with type " << msg.plan_type.type << " but ignored."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received mobility request with type " << msg.plan_type.type << " but ignored."); return MobilityRequestResponse::NO_RESPONSE; } @@ -2080,16 +2076,16 @@ namespace platoon_strategic_ihp // ------ 4. Mobility response callback ------ // // Mobility response callback for all states. - void PlatoonStrategicIHPPlugin::mob_resp_cb(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg) { // Firstly, check eligibility of the received message. bool isCurrPlanValid = pm_.current_plan.valid; // Check if current plan is still valid (i.e., not timed out). - bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId; // Check if plan Id matches. - bool isFromTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; // Check if expected peer ID and sender ID matches. - ROS_DEBUG_STREAM("mob_resp_cb: isCurrPlanValid = " << isCurrPlanValid << ", isForCurrentPlan = " << + bool isForCurrentPlan = msg->m_header.plan_id == pm_.current_plan.planId; // Check if plan Id matches. + bool isFromTargetVehicle = msg->m_header.sender_id == pm_.current_plan.peerId; // Check if expected peer ID and sender ID matches. + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "mob_resp_cb: isCurrPlanValid = " << isCurrPlanValid << ", isForCurrentPlan = " << isForCurrentPlan << ", isFromTargetVehicle = " << isFromTargetVehicle); - ROS_DEBUG_STREAM("sender ID = " << msg.m_header.sender_id << ", current peer ID = " << pm_.current_plan.peerId); - ROS_DEBUG_STREAM("incoming plan ID = " << msg.m_header.plan_id << "current plan ID = " << pm_.current_plan.planId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "sender ID = " << msg->m_header.sender_id << ", current peer ID = " << pm_.current_plan.peerId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "incoming plan ID = " << msg->m_header.plan_id << "current plan ID = " << pm_.current_plan.planId); if (!(isCurrPlanValid && isForCurrentPlan && isFromTargetVehicle)) { @@ -2097,67 +2093,67 @@ namespace platoon_strategic_ihp * If any of the three condition (i.e., isCurrPlanValid, isForCurrentPlan and isFromTargetVehicle) * was not satisfied, return ignore as this message was not intended for the host. */ - ROS_DEBUG_STREAM(" Ignore the received response message as it was not intended for the host vehicle."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " Ignore the received response message as it was not intended for the host vehicle."); return; } else if (pm_.current_platoon_state == PlatoonState::LEADER) { - mob_resp_cb_leader(msg); + mob_resp_cb_leader(*msg); } else if (pm_.current_platoon_state == PlatoonState::FOLLOWER) { - mob_resp_cb_follower(msg); + mob_resp_cb_follower(*msg); } else if (pm_.current_platoon_state == PlatoonState::CANDIDATEFOLLOWER) { - mob_resp_cb_candidatefollower(msg); + mob_resp_cb_candidatefollower(*msg); } else if (pm_.current_platoon_state == PlatoonState::LEADERWAITING) { - mob_resp_cb_leaderwaiting(msg); + mob_resp_cb_leaderwaiting(*msg); } else if (pm_.current_platoon_state == PlatoonState::STANDBY) { - mob_resp_cb_standby(msg); + mob_resp_cb_standby(*msg); } // UCLA: add leader aboorting else if (pm_.current_platoon_state == PlatoonState::LEADERABORTING) { - mob_resp_cb_leaderaborting(msg); + mob_resp_cb_leaderaborting(*msg); } //UCLA: add candidate leader else if (pm_.current_platoon_state == PlatoonState::CANDIDATELEADER) { - mob_resp_cb_candidateleader(msg); + mob_resp_cb_candidateleader(*msg); } // UCLA: add lead with operation for cut-in join else if (pm_.current_platoon_state == PlatoonState::LEADWITHOPERATION) { - mob_resp_cb_leadwithoperation(msg); + mob_resp_cb_leadwithoperation(*msg); } // UCLA: add prepare to join for cut-in join else if (pm_.current_platoon_state == PlatoonState::PREPARETOJOIN) { - mob_resp_cb_preparetojoin(msg); + mob_resp_cb_preparetojoin(*msg); } // TODO: Place holder for departure. } - void PlatoonStrategicIHPPlugin::mob_resp_cb_standby(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_standby(const carma_v2x_msgs::msg::MobilityResponse& msg) { // In standby state, it will not send out any requests so it will also ignore all responses - ROS_DEBUG_STREAM("STANDBY state does nothing with msg from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "STANDBY state does nothing with msg from " << msg.m_header.sender_id); } - void PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_candidatefollower(const carma_v2x_msgs::msg::MobilityResponse& msg) { - ROS_DEBUG_STREAM("Callback for candidate follower "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Callback for candidate follower "); // Check if current plan is still valid (i.e., not timed out) if (pm_.current_plan.valid) { bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId; - ROS_DEBUG_STREAM("isForCurrentPlan " << isForCurrentPlan); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isForCurrentPlan " << isForCurrentPlan); // Check the response is received correctly (i.e., host vehicle is the desired receiver). if (isForCurrentPlan) @@ -2167,13 +2163,13 @@ namespace platoon_strategic_ihp // We change to follower state and start to actually follow that leader // The platoon manager also need to change the platoon Id to the one that the target leader is using pm_.current_platoon_state = PlatoonState::FOLLOWER; - ROS_DEBUG_STREAM("pm_.currentPlatoonID: " << pm_.currentPlatoonID); - ROS_DEBUG_STREAM("pm_.targetPlatoonID: " << pm_.currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID: " << pm_.currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.targetPlatoonID: " << pm_.currentPlatoonID); if (pm_.targetPlatoonID.compare(pm_.dummyID) != 0) { pm_.currentPlatoonID = pm_.targetPlatoonID; - ROS_DEBUG_STREAM("pm_.currentPlatoonID now: " << pm_.currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID now: " << pm_.currentPlatoonID); pm_.resetNeighborPlatoon(); } else @@ -2182,15 +2178,15 @@ namespace platoon_strategic_ihp } pm_.changeFromLeaderToFollower(pm_.currentPlatoonID, msg.m_header.sender_id); - ROS_DEBUG_STREAM("The leader " << msg.m_header.sender_id << " agreed on our join. Change to follower state."); - ROS_WARN("changed to follower"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The leader " << msg.m_header.sender_id << " agreed on our join. Change to follower state."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"changed to follower"); pm_.clearActionPlan(); } else { // We change back to normal leader state and try to join other platoons - ROS_DEBUG_STREAM("The leader " << msg.m_header.sender_id << " does not agree on our join. Change back to leader state."); - ROS_DEBUG_STREAM("Trying again.."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The leader " << msg.m_header.sender_id << " does not agree on our join. Change back to leader state."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Trying again.."); // join plan failed, but we still need the peerid pm_.current_plan.valid = false; @@ -2204,26 +2200,26 @@ namespace platoon_strategic_ihp } else { - ROS_DEBUG_STREAM("Ignore received response message because it is not for the current plan."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because it is not for the current plan."); } } else { - ROS_DEBUG_STREAM("Ignore received response message because we are not in any negotiation process."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because we are not in any negotiation process."); } } - void PlatoonStrategicIHPPlugin::mob_resp_cb_leaderwaiting(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_leaderwaiting(const carma_v2x_msgs::msg::MobilityResponse& msg) { /** * Leader waiting is the state to check joining vehicle is in proper position * and to prevent platoon leader from receiving messages from other CAVs in leader state. * There was no response involved in this state, hence no action needed in this section. */ - ROS_DEBUG_STREAM("LEADERWAITING state does nothing with msg from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADERWAITING state does nothing with msg from " << msg.m_header.sender_id); } - void PlatoonStrategicIHPPlugin::mob_resp_cb_follower(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_follower(const carma_v2x_msgs::msg::MobilityResponse& msg) { /** * UCLA Note: @@ -2238,28 +2234,28 @@ namespace platoon_strategic_ihp */ // UCLA: read plan type - cav_msgs::PlanType plan_type = msg.plan_type; + carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type; // UCLA: determine joining type - bool isFrontJoin = (plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT); + bool isFrontJoin = (plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT); //TODO: when would this code block ever be used? A normal follower would have to talk to a front joiner. // UCLA: add response so follower can change to candidate follower, then change leader if (isFrontJoin && msg.is_accepted) { // if frontal join is accepted, change followers to candidate follower to update leader - ROS_DEBUG_STREAM("Received positive response for front-join plan id = " << pm_.current_plan.planId); - ROS_DEBUG_STREAM("Change to CandidateFollower state and prepare to update platoon information"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for front-join plan id = " << pm_.current_plan.planId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to CandidateFollower state and prepare to update platoon information"); // Change to candidate follower state and request a new plan to catch up with the front platoon pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; } // TODO: Place holder for follower departure. } // UCLA: add conditions to account for frontal join states (candidate follower) - void PlatoonStrategicIHPPlugin::mob_resp_cb_leader(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_leader(const carma_v2x_msgs::msg::MobilityResponse& msg) { /** * UCLA implementation note: @@ -2276,23 +2272,22 @@ namespace platoon_strategic_ihp */ // UCLA: read plan type - cav_msgs::PlanType plan_type = msg.plan_type; - ROS_DEBUG_STREAM("plan_type = " << plan_type); - ROS_DEBUG_STREAM("plan_type.type = " << plan_type.type); + carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "plan_type.type = " << plan_type.type); // UCLA: determine joining type - bool isCutInJoin = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN && !config_.test_front_join; - bool isRearJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_AT_REAR && !config_.test_front_join; - bool isFrontJoin = plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT || config_.test_front_join; - ROS_DEBUG_STREAM("Joining type: isRearJoin = " << isRearJoin); - ROS_DEBUG_STREAM("Joining type: isFrontJoin = " << isFrontJoin); - ROS_DEBUG_STREAM("Joining type: isCutInJoin = " << isCutInJoin); + bool isCutInJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN && !config_.test_front_join; + bool isRearJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_AT_REAR && !config_.test_front_join; + bool isFrontJoin = plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT || config_.test_front_join; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining type: isRearJoin = " << isRearJoin); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining type: isFrontJoin = " << isFrontJoin); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Joining type: isCutInJoin = " << isCutInJoin); // Check if current plan is still valid (i.e., not timed out). if (pm_.current_plan.valid) { - ROS_DEBUG_STREAM("My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id); - ROS_DEBUG_STREAM("Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id); // Check the response is received correctly (i.e., host vehicle is the desired receiver). if (pm_.current_plan.planId == msg.m_header.plan_id && pm_.current_plan.peerId == msg.m_header.sender_id) @@ -2300,24 +2295,24 @@ namespace platoon_strategic_ihp // rear join if (isRearJoin && msg.is_accepted) { - ROS_DEBUG_STREAM("Received positive response for plan id = " << pm_.current_plan.planId); - ROS_DEBUG_STREAM("Change to CandidateFollower state and notify trajectory failure in order to replan"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for plan id = " << pm_.current_plan.planId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to CandidateFollower state and notify trajectory failure in order to replan"); // Change to candidate follower state and wait to catch up with the front platoon pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; pm_.current_plan.valid = false; //but leave peerId intact for use in second request } // UCLA: frontal join (candidate leader, inherited from leaderwaiting) else if (isFrontJoin && msg.is_accepted) { - ROS_DEBUG_STREAM("Received positive response for plan id = " << pm_.current_plan.planId); - ROS_DEBUG_STREAM("Change to CandidateLeader state and prepare to become new leader. "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for plan id = " << pm_.current_plan.planId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to CandidateLeader state and prepare to become new leader. "); // Change to candidate leader and idle pm_.current_platoon_state = PlatoonState::CANDIDATELEADER; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; pm_.current_plan.valid = false; //but leave peerId intact for use in second request // Set the platoon ID to that of the target platoon even though we haven't yet joined; @@ -2329,13 +2324,13 @@ namespace platoon_strategic_ihp // UCLA: CutIn join else if (isCutInJoin && msg.is_accepted) { - ROS_DEBUG_STREAM("Received positive response for plan id = " << pm_.current_plan.planId); - ROS_DEBUG_STREAM("Change to Prepare to join state and prepare to change lane. "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received positive response for plan id = " << pm_.current_plan.planId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Change to Prepare to join state and prepare to change lane. "); // Change to candidate leader and idle pm_.current_platoon_state = PlatoonState::PREPARETOJOIN; pm_.neighbor_platoon_leader_id_ = msg.m_header.sender_id; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; pm_.current_plan.valid = false; //but leave peerId intact for use in second request } @@ -2345,11 +2340,11 @@ namespace platoon_strategic_ihp //TODO future: add logic here to allow two platoons to join together // Keep the leader idling, since this must be a bogus response - ROS_WARN_STREAM("Host received response for joining vehicles, remain idling as the host is a current platoon leader."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Host received response for joining vehicles, remain idling as the host is a current platoon leader."); } else { - ROS_DEBUG_STREAM("Received negative response for plan id = " << pm_.current_plan.planId << ". Resetting plan & platoon info."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Received negative response for plan id = " << pm_.current_plan.planId << ". Resetting plan & platoon info."); // Forget about the previous plan totally pm_.clearActionPlan(); pm_.resetHostPlatoon(); @@ -2357,16 +2352,16 @@ namespace platoon_strategic_ihp } else { - ROS_DEBUG_STREAM("Ignore the response message because planID match: " << (pm_.current_plan.planId == msg.m_header.plan_id)); - ROS_DEBUG_STREAM("My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id); - ROS_DEBUG_STREAM("And peer id match " << (pm_.current_plan.peerId == msg.m_header.sender_id)); - ROS_DEBUG_STREAM("Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore the response message because planID match: " << (pm_.current_plan.planId == msg.m_header.plan_id)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "My plan id = " << pm_.current_plan.planId << " and response plan Id = " << msg.m_header.plan_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "And peer id match " << (pm_.current_plan.peerId == msg.m_header.sender_id)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Expected peer id = " << pm_.current_plan.peerId << " and response sender Id = " << msg.m_header.sender_id); } } } // UCLA: response for leader aborting (inherited from candidate follower) - void PlatoonStrategicIHPPlugin::mob_resp_cb_leaderaborting(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_leaderaborting(const carma_v2x_msgs::msg::MobilityResponse& msg) { /** * UCLA implementation note: @@ -2379,30 +2374,30 @@ namespace platoon_strategic_ihp * corresponding join request will be send out by the previos leader. */ - ROS_DEBUG_STREAM("Callback for leader aborting !"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Callback for leader aborting !"); // Check if current plan is still valid (i.e., not timed out). if (pm_.current_plan.valid) { bool isForCurrentPlan = msg.m_header.plan_id == pm_.current_plan.planId; - bool isForFrontJoin = msg.plan_type.type == cav_msgs::PlanType::PLATOON_FRONT_JOIN; - - if (msg.plan_type.type == cav_msgs::PlanType::UNKNOWN){ - ROS_DEBUG_STREAM("*** plan type UNKNOWN"); - }else if (msg.plan_type.type == cav_msgs::PlanType::JOIN_PLATOON_FROM_FRONT){ - ROS_DEBUG_STREAM("*** plan type JOIN_PLATOON_FROM_FRONT"); - }else if (msg.plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN){ - ROS_DEBUG_STREAM("*** plan type PLATOON_CUT_IN_JOIN"); + bool isForFrontJoin = msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_FRONT_JOIN; + + if (msg.plan_type.type == carma_v2x_msgs::msg::PlanType::UNKNOWN){ + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type UNKNOWN"); + }else if (msg.plan_type.type == carma_v2x_msgs::msg::PlanType::JOIN_PLATOON_FROM_FRONT){ + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type JOIN_PLATOON_FROM_FRONT"); + }else if (msg.plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN){ + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type PLATOON_CUT_IN_JOIN"); }else { - ROS_DEBUG_STREAM("*** plan type not captured."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "*** plan type not captured."); } bool isFromTargetVehicle = msg.m_header.sender_id == pm_.current_plan.peerId; - ROS_DEBUG_STREAM("msg.header.sender_id " << msg.m_header.sender_id); - ROS_DEBUG_STREAM("Plan Type " << msg.plan_type.type); - ROS_DEBUG_STREAM("isForFrontJoin " << isForFrontJoin); - ROS_DEBUG_STREAM("isForCurrentPlan " << isForCurrentPlan); - ROS_DEBUG_STREAM("isFromTargetVehicle " << isFromTargetVehicle); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "msg.m_header.sender_id " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Plan Type " << msg.plan_type.type); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isForFrontJoin " << isForFrontJoin); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isForCurrentPlan " << isForCurrentPlan); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isFromTargetVehicle " << isFromTargetVehicle); // Check the response is received correctly (i.e., host vehicle is the desired receiver). if (isForCurrentPlan && isFromTargetVehicle && isForFrontJoin) @@ -2413,8 +2408,8 @@ namespace platoon_strategic_ihp // The platoon manager also need to change the platoon Id to the one that the target leader is using pm_.current_platoon_state = PlatoonState::FOLLOWER; pm_.changeFromLeaderToFollower(pm_.currentPlatoonID, msg.m_header.sender_id); - ROS_DEBUG_STREAM("The new leader " << msg.m_header.sender_id << " agreed on the frontal join. Change to follower state."); - ROS_WARN("changed to follower"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The new leader " << msg.m_header.sender_id << " agreed on the frontal join. Change to follower state."); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"changed to follower"); // reset leader aborting request marker numLeaderAbortingCalls_ = 0; @@ -2422,13 +2417,13 @@ namespace platoon_strategic_ihp else { // We change back to normal leader state - ROS_DEBUG_STREAM("The new leader " << msg.m_header.sender_id << " does not agree on the frontal join. Change back to leader state."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The new leader " << msg.m_header.sender_id << " does not agree on the frontal join. Change back to leader state."); pm_.current_platoon_state = PlatoonState::LEADER; // We were already leading a platoon, so don't erase any of that info. But we need to remove the erstwhile candidate // leader from our platoon roster; we know it is in position 0, so just remove that element if (!pm_.removeMember(0)) { - ROS_DEBUG_STREAM("Failed to remove candidate leader from the platoon!"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Failed to remove candidate leader from the platoon!"); } } @@ -2437,71 +2432,68 @@ namespace platoon_strategic_ihp } else { - ROS_DEBUG_STREAM("Ignore received response message because it is not for the current plan."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because it is not for the current plan."); } } else { - ROS_DEBUG_STREAM("Ignore received response message because we are not in any negotiation process."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignore received response message because we are not in any negotiation process."); } } // UCLA: response for candidate leader (inherited from leader waiting) - void PlatoonStrategicIHPPlugin::mob_resp_cb_candidateleader(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_candidateleader(const carma_v2x_msgs::msg::MobilityResponse& msg) { - ROS_DEBUG_STREAM("CANDIDATELEADER state does nothing with msg from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CANDIDATELEADER state does nothing with msg from " << msg.m_header.sender_id); } // UCLA: response callback for lead with operation - void PlatoonStrategicIHPPlugin::mob_resp_cb_leadwithoperation(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_leadwithoperation(const carma_v2x_msgs::msg::MobilityResponse& msg) { - ROS_DEBUG_STREAM("LEADWITHOPERATION state does nothing with msg from " << msg.m_header.sender_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LEADWITHOPERATION state does nothing with msg from " << msg.m_header.sender_id); } // UCLA: response callback for prepare to join (inherited from leader waiting) - void PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin(const cav_msgs::MobilityResponse& msg) + void PlatoonStrategicIHPPlugin::mob_resp_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityResponse& msg) { /* If leader notify the member to slow down and ACK the request, start to check the gap and change lane when gap is large enough */ - cav_msgs::PlanType plan_type = msg.plan_type; - bool isCreatingGap = plan_type.type == cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; - bool isFinishLaneChangeFront = plan_type.type == cav_msgs::PlanType::CUT_IN_FRONT_DONE; - bool isFinishLaneChangeMidorRear = plan_type.type == cav_msgs::PlanType::CUT_IN_MID_OR_REAR_DONE; - ROS_DEBUG_STREAM("isCreatingGap = " << isCreatingGap << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_); + carma_v2x_msgs::msg::PlanType plan_type = msg.plan_type; + bool isCreatingGap = plan_type.type == carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN; + bool isFinishLaneChangeFront = plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE; + bool isFinishLaneChangeMidorRear = plan_type.type == carma_v2x_msgs::msg::PlanType::CUT_IN_MID_OR_REAR_DONE; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isCreatingGap = " << isCreatingGap << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_); if (!msg.is_accepted) { - ROS_DEBUG_STREAM("Request " << msg.m_header.plan_id << " was rejected by leader."); - ROS_DEBUG_STREAM("Action Plan reset."); - ROS_DEBUG_STREAM("Trying again...."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Request " << msg.m_header.plan_id << " was rejected by leader."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Action Plan reset."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Trying again...."); pm_.current_plan.valid = false; pm_.current_platoon_state = PlatoonState::LEADER; return; } // UCLA: Create Gap or perform a rear join (no gap creation necessary) - ROS_DEBUG_STREAM("pm_.is_neighbor_record_complete_ " << pm_.is_neighbor_record_complete_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.is_neighbor_record_complete_ " << pm_.is_neighbor_record_complete_); if (isCreatingGap && pm_.is_neighbor_record_complete_) { - // task 1: check gap - double cut_in_gap = pm_.getCutInGap(target_join_index_, current_downtrack_); - // cut-in gap not needed for front and rear join, so ignored // task 2: set indicator if gap is safe safeToLaneChange_ = true; - ROS_DEBUG_STREAM("Gap is now sufficiently large."); - ROS_DEBUG_STREAM("in mob_resp_cb safeToLaneChange_: " << safeToLaneChange_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Gap is now sufficiently large."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "in mob_resp_cb safeToLaneChange_: " << safeToLaneChange_); // task 3: notify gap-rear vehicle to stop slowing down - cav_msgs::MobilityRequest request; + carma_v2x_msgs::msg::MobilityRequest request; request.m_header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); request.m_header.recipient_id = pm_.current_plan.peerId; request.m_header.sender_id = config_.vehicleID; - request.m_header.timestamp = ros::Time::now().toNSec() / 1000000;; + request.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000;; // UCLA: A new plan type to stop creat gap. - request.plan_type.type = cav_msgs::PlanType::STOP_CREATE_GAP; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::STOP_CREATE_GAP; request.strategy = PLATOONING_STRATEGY; request.urgency = 50; request.location = pose_to_ecef(pose_msg_); @@ -2517,21 +2509,21 @@ namespace platoon_strategic_ihp request.strategy_params = fmter.str(); mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Published Mobility Candidate-Join request to the leader to stop creating gap"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility Candidate-Join request to the leader to stop creating gap"); } // UCLA: Revert to same-lane for cut-in front else if (isFinishLaneChangeFront) { - ROS_DEBUG_STREAM("Cut-in from front lane change finished, the joining vehicle revert to same-lane maneuver."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from front lane change finished, the joining vehicle revert to same-lane maneuver."); pm_.current_platoon_state = PlatoonState::CANDIDATELEADER; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; - ROS_DEBUG_STREAM("pm_.currentPlatoonID: " << pm_.currentPlatoonID); - ROS_DEBUG_STREAM("pm_.targetPlatoonID: " << pm_.targetPlatoonID); + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID: " << pm_.currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.targetPlatoonID: " << pm_.targetPlatoonID); if (pm_.targetPlatoonID.compare(pm_.dummyID) != 0) { pm_.currentPlatoonID = pm_.targetPlatoonID; - ROS_DEBUG_STREAM("pm_.currentPlatoonID now: " << pm_.currentPlatoonID); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.currentPlatoonID now: " << pm_.currentPlatoonID); } pm_.current_plan.valid = false; //but leave peerId intact for use in second request @@ -2540,16 +2532,16 @@ namespace platoon_strategic_ihp // UCLA: Revert to same-lane operation for cut-in from middle/rear else if (isFinishLaneChangeMidorRear) { - ROS_DEBUG_STREAM("Cut-in from mid or rear, the lane change finished, the joining vehicle revert to same-lane maneuver."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cut-in from mid or rear, the lane change finished, the joining vehicle revert to same-lane maneuver."); pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; - candidatestateStartTime = ros::Time::now().toNSec() / 1000000; + candidatestateStartTime = timer_factory_->now().nanoseconds() / 1000000; pm_.current_plan.valid = false; //but leave peerId intact for use in second request } else { - ROS_DEBUG_STREAM("End of mob_resp_cb_preparetojoin"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "End of mob_resp_cb_preparetojoin"); } } @@ -2558,33 +2550,33 @@ namespace platoon_strategic_ihp // ------ 5. response types ------- // // ACK --> yes,accept host as member; NACK --> no, cannot accept host as member - void PlatoonStrategicIHPPlugin::mob_req_cb(const cav_msgs::MobilityRequest& msg) + void PlatoonStrategicIHPPlugin::mob_req_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg) { // Ignore messages as long as host vehicle is stopped if (current_speed_ < config_.minPlatooningSpeed) { - ROS_DEBUG_STREAM("Ignoring message since host speed is below platooning speed."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring message since host speed is below platooning speed."); return; } // Check that this is a message about platooning (could be from some other Carma activity nearby) - std::string strategy = msg.strategy; + std::string strategy = msg->strategy; if (strategy.rfind(PLATOONING_STRATEGY, 0) != 0) { - ROS_DEBUG_STREAM("Ignoring mobility operation message for " << strategy << " strategy."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Ignoring mobility operation message for " << strategy << " strategy."); return; } - cav_msgs::MobilityResponse response; + carma_v2x_msgs::msg::MobilityResponse response; response.m_header.sender_id = config_.vehicleID; - response.m_header.recipient_id = msg.m_header.sender_id; - response.m_header.plan_id = msg.m_header.plan_id; - response.m_header.timestamp = ros::Time::now().toNSec() / 1000000; + response.m_header.recipient_id = msg->m_header.sender_id; + response.m_header.plan_id = msg->m_header.plan_id; + response.m_header.timestamp = timer_factory_->now().nanoseconds() / 1000000; // UCLA: add plantype in response - response.plan_type.type = msg.plan_type.type; + response.plan_type.type = msg->plan_type.type; - MobilityRequestResponse req_response = handle_mob_req(msg); + MobilityRequestResponse req_response = handle_mob_req(*msg); if (req_response == MobilityRequestResponse::ACK) { response.is_accepted = true; @@ -2597,7 +2589,7 @@ namespace platoon_strategic_ihp } else { - ROS_DEBUG_STREAM(" NO response to mobility request. "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), " NO response to mobility request. "); } } @@ -2606,29 +2598,32 @@ namespace platoon_strategic_ihp void PlatoonStrategicIHPPlugin::run_leader_waiting() { - ROS_DEBUG_STREAM("Run LeaderWaiting State "); - long tsStart = ros::Time::now().toNSec() / 1000000; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Run LeaderWaiting State "); + long tsStart = timer_factory_->now().nanoseconds() / 1000000; // Task 1 if (tsStart - waitingStartTime > waitingStateTimeout * 1000) { //TODO if the current state timeouts, we need to have a kind of ABORT message to inform the applicant - ROS_DEBUG_STREAM("LeaderWaitingState is timeout, changing back to PlatoonLeaderState."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "LeaderWaitingState is timeout, changing back to PlatoonLeaderState."); pm_.current_platoon_state = PlatoonState::LEADER; pm_.clearActionPlan(); } // Task 2 - cav_msgs::MobilityOperation status; + carma_v2x_msgs::msg::MobilityOperation status; status = composeMobilityOperationLeaderWaiting(); mobility_operation_publisher_(status); - ROS_DEBUG_STREAM("publish status message"); - long tsEnd = ros::Time::now().toNSec() / 1000000; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "publish status message"); + long tsEnd = timer_factory_->now().nanoseconds() / 1000000; long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration / 1000).sleep(); + + // TODO this solution is not sim-time complient and should be replaced with one which is when possible + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888 + std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration)); } void PlatoonStrategicIHPPlugin::run_leader() { - unsigned long tsStart = ros::Time::now().toNSec() / 1000000; + unsigned long tsStart = timer_factory_->now().nanoseconds() / 1000000; // If vehicle is not rolling then return if (current_speed_ <= STOPPED_SPEED) @@ -2638,14 +2633,14 @@ namespace platoon_strategic_ihp // Task 1: heart beat timeout: send INFO mob_op bool isTimeForHeartBeat = tsStart - prevHeartBeatTime_ >= infoMessageInterval_; - ROS_DEBUG_STREAM("time since last heart beat: " << tsStart - prevHeartBeatTime_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time since last heart beat: " << tsStart - prevHeartBeatTime_); if (isTimeForHeartBeat) { - cav_msgs::MobilityOperation infoOperation; + carma_v2x_msgs::msg::MobilityOperation infoOperation; infoOperation = composeMobilityOperationLeader(OPERATION_INFO_TYPE); mobility_operation_publisher_(infoOperation); - prevHeartBeatTime_ = ros::Time::now().toNSec() / 1000000; - ROS_DEBUG_STREAM("Published heart beat platoon INFO mobility operation message"); + prevHeartBeatTime_ = timer_factory_->now().nanoseconds() / 1000000; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published heart beat platoon INFO mobility operation message"); } // Task 3: plan time out, check if any current join plan is still valid (i.e., not timed out). @@ -2654,27 +2649,29 @@ namespace platoon_strategic_ihp bool isCurrentPlanTimeout = tsStart - pm_.current_plan.planStartTime > NEGOTIATION_TIMEOUT; if (isCurrentPlanTimeout) { - ROS_DEBUG_STREAM("Give up current on waiting plan with planId: " << pm_.current_plan.planId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Give up current on waiting plan with planId: " << pm_.current_plan.planId); pm_.clearActionPlan(); } } // Task 4: STATUS msgs bool hasFollower = pm_.getHostPlatoonSize() > 1 || config_.test_cutin_join; - ROS_DEBUG_STREAM("hasFollower" << hasFollower); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "hasFollower" << hasFollower); // if has follower, publish platoon message as STATUS mob_op if (hasFollower) { - cav_msgs::MobilityOperation statusOperation; + carma_v2x_msgs::msg::MobilityOperation statusOperation; statusOperation = composeMobilityOperationLeader(OPERATION_STATUS_TYPE); // mob_op_pub_.publish(statusOperation); mobility_operation_publisher_(statusOperation); - ROS_DEBUG_STREAM("Published platoon STATUS operation message as a Leader with Follower"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message as a Leader with Follower"); } - long tsEnd = ros::Time::now().toNSec() / 1000000; + long tsEnd = timer_factory_->now().nanoseconds() / 1000000; long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration / 1000).sleep(); + // TODO this solution is not sim-time complient and should be replaced with one which is when possible + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888 + std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration)); // Job 5: Dissoleve request. // TODO: Place holder for departure. Need to change to departing state and tracking departng ID. @@ -2683,15 +2680,15 @@ namespace platoon_strategic_ihp void PlatoonStrategicIHPPlugin::run_follower() { - ROS_DEBUG_STREAM("run follower"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "run follower"); // This is a interrupted-safe loop. // This loop has four tasks: // 1. Check the state start time, if it exceeds a limit it will give up current plan and change back to leader state // 2. Abort current request if we wait for long enough time for response from leader and change back to leader state - long tsStart = ros::Time::now().toNSec() / 1000000; + long tsStart = timer_factory_->now().nanoseconds() / 1000000; // Job 1 - cav_msgs::MobilityOperation status; + carma_v2x_msgs::msg::MobilityOperation status; status = composeMobilityOperationFollower(); mobility_operation_publisher_(status); // Job 2 @@ -2702,7 +2699,7 @@ namespace platoon_strategic_ihp noLeaderUpdatesCounter++; if (noLeaderUpdatesCounter >= LEADER_TIMEOUT_COUNTER_LIMIT) { - ROS_DEBUG_STREAM("noLeaderUpdatesCounter = " << noLeaderUpdatesCounter << " and change to leader state"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "noLeaderUpdatesCounter = " << noLeaderUpdatesCounter << " and change to leader state"); pm_.changeFromFollowerToLeader(); pm_.current_platoon_state = PlatoonState::LEADER; noLeaderUpdatesCounter = 0; @@ -2713,9 +2710,11 @@ namespace platoon_strategic_ihp // reset counter to zero when we get updates again noLeaderUpdatesCounter = 0; } - long tsEnd = ros::Time::now().toNSec() / 1000000; + long tsEnd = timer_factory_->now().nanoseconds() / 1000000; long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration / 1000).sleep(); + // TODO this solution is not sim-time complient and should be replaced with one which is when possible + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888 + std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration)); // Job 3: Dissoleve request. //TODO: set departure indicator @@ -2724,15 +2723,15 @@ namespace platoon_strategic_ihp void PlatoonStrategicIHPPlugin::run_candidate_follower() { - long tsStart = ros::Time::now().toNSec() / 1000000; + long tsStart = timer_factory_->now().nanoseconds() / 1000000; // Task 1: state timeout bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000; - ROS_DEBUG_STREAM("timeout1: " << tsStart - candidatestateStartTime); - ROS_DEBUG_STREAM("waitingStateTimeout: " << waitingStateTimeout * 1000); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout1: " << tsStart - candidatestateStartTime); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "waitingStateTimeout: " << waitingStateTimeout * 1000); if (isCurrentStateTimeout) { - ROS_DEBUG_STREAM("The current candidate follower state is timeout. Change back to leader state."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current candidate follower state is timeout. Change back to leader state."); pm_.current_platoon_state = PlatoonState::LEADER; pm_.clearActionPlan(); } @@ -2740,16 +2739,16 @@ namespace platoon_strategic_ihp // Task 2: plan timeout, check if current plan is still valid (i.e., not timed out). if (pm_.current_plan.valid) { - ROS_DEBUG_STREAM("pm_.current_plan.planStartTime: " << pm_.current_plan.planStartTime); - ROS_DEBUG_STREAM("timeout2: " << tsStart - pm_.current_plan.planStartTime); - ROS_DEBUG_STREAM("NEGOTIATION_TIMEOUT: " << NEGOTIATION_TIMEOUT); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.current_plan.planStartTime: " << pm_.current_plan.planStartTime); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout2: " << tsStart - pm_.current_plan.planStartTime); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "NEGOTIATION_TIMEOUT: " << NEGOTIATION_TIMEOUT); bool isPlanTimeout = tsStart - pm_.current_plan.planStartTime > NEGOTIATION_TIMEOUT; if (isPlanTimeout) { pm_.current_platoon_state = PlatoonState::LEADER; pm_.clearActionPlan(); - ROS_DEBUG_STREAM("The current plan did not receive any response. Abort and change to leader state."); - ROS_DEBUG_STREAM("Changed the state back to Leader"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current plan did not receive any response. Abort and change to leader state."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Changed the state back to Leader"); } } @@ -2760,37 +2759,37 @@ namespace platoon_strategic_ihp if (!pm_.neighbor_platoon_.empty()) { currentGap = pm_.neighbor_platoon_.back().vehiclePosition - current_downtrack_; - ROS_DEBUG_STREAM("curent gap calculated from back of neighbor platoon: " << currentGap); - ROS_DEBUG_STREAM("pm_.neighbor_platoon_.back().vehiclePosition " << pm_.neighbor_platoon_.back().vehiclePosition); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "curent gap calculated from back of neighbor platoon: " << currentGap); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.neighbor_platoon_.back().vehiclePosition " << pm_.neighbor_platoon_.back().vehiclePosition); } else { currentGap = pm_.getDistanceToPredVehicle(); - ROS_DEBUG_STREAM("curent gap when there is no neighbor platoon: " << currentGap); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "curent gap when there is no neighbor platoon: " << currentGap); } - ROS_DEBUG_STREAM("Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " ms"); - ROS_DEBUG_STREAM("Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m"); - ROS_DEBUG_STREAM("The current gap from radar is " << currentGap << " m"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " ms"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current gap from radar is " << currentGap << " m"); if (currentGap <= maxJoinGap && !pm_.current_plan.valid) { - cav_msgs::MobilityRequest request; + carma_v2x_msgs::msg::MobilityRequest request; std::string planId = boost::uuids::to_string(boost::uuids::random_generator()()); - long currentTime = ros::Time::now().toNSec() / 1000000; + long currentTime = timer_factory_->now().nanoseconds() / 1000000; request.m_header.plan_id = planId; request.m_header.recipient_id = pm_.current_plan.peerId; request.m_header.sender_id = config_.vehicleID; request.m_header.timestamp = currentTime; - request.plan_type.type = cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_FOLLOWER_JOIN; request.strategy = PLATOONING_STRATEGY; request.strategy_params = ""; //params will not be read by receiver since this is 2nd msg in sequence request.urgency = 50; request.location = pose_to_ecef(pose_msg_); mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Published Mobility Candidate-Join request to the leader"); - ROS_DEBUG_STREAM("current plan peer id: " << pm_.current_plan.peerId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility Candidate-Join request to the leader"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current plan peer id: " << pm_.current_plan.peerId); // Update the local record of the new activity plan and now establish that we have a platoon plan as well, // which allows us to start sending necessary op STATUS messages @@ -2807,16 +2806,18 @@ namespace platoon_strategic_ihp // Don't want to do this until after the above MobReq message is delivered, otherwise recipient will double-count us in their platoon if (++candidate_follower_delay_count_ > 2) { - cav_msgs::MobilityOperation status; + carma_v2x_msgs::msg::MobilityOperation status; status = composeMobilityOperationCandidateFollower(); mobility_operation_publisher_(status); - ROS_DEBUG_STREAM("Published platoon STATUS operation message as Candidate Follower"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message as Candidate Follower"); } } - long tsEnd = ros::Time::now().toNSec() / 1000000; + long tsEnd = timer_factory_->now().nanoseconds() / 1000000; long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration / 1000).sleep(); + // TODO this solution is not sim-time complient and should be replaced with one which is when possible + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888 + std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration)); } // UCLA: frontal join state (inherit from candidate follower: prepare to give up leading state and accept the new leader) @@ -2827,14 +2828,14 @@ namespace platoon_strategic_ihp 1. this function send step plan type: "PLATOON_FRONT_JOIN" 2. the sender of the plan (host vehicle) is the previous leader, it wil prepare to follow front joiner (new leader) */ - long tsStart = ros::Time::now().toNSec() / 1000000; + long tsStart = timer_factory_->now().nanoseconds() / 1000000; // Task 1: state timeout bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000; - ROS_DEBUG_STREAM("timeout1: " << tsStart - candidatestateStartTime); - ROS_DEBUG_STREAM("waitingStateTimeout: " << waitingStateTimeout * 1000); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout1: " << tsStart - candidatestateStartTime); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "waitingStateTimeout: " << waitingStateTimeout * 1000); if (isCurrentStateTimeout) { - ROS_DEBUG_STREAM("The current leader aborting state is timeout. Change back to leader state."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current leader aborting state is timeout. Change back to leader state."); pm_.current_platoon_state = PlatoonState::LEADER; //clear plan validity & end; leave platoon info alone, as we may still be leading a valid platoon @@ -2851,9 +2852,9 @@ namespace platoon_strategic_ihp // preceding host is the candidate joiner. For this code to work, it depends on the candidate to publish // mobility operation STATUS messages so that host can include it in the pm_ platoon membership. double currentGap = pm_.getDistanceToPredVehicle(); //returns 0 if we haven't received op STATUS from joiner yet - ROS_DEBUG_STREAM("Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " m"); - ROS_DEBUG_STREAM("Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m"); - ROS_DEBUG_STREAM("The current gap to joiner is " << currentGap << " m"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Based on desired join time gap, the desired join distance gap is " << desiredJoinGap2 << " m"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Since we have max allowed gap as " << config_.desiredJoinGap << " m then max join gap became " << maxJoinGap << " m"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current gap to joiner is " << currentGap << " m"); // NOTE: The front join depends upon the joiner to publish op STATUS messages with this platoon's ID, then host receives at least one // and thereby adds the joiner to the platoon record. This process requires host's mob_req_cb_leader() to ACK the join request, then @@ -2866,13 +2867,13 @@ namespace platoon_strategic_ihp // Check if gap is big enough and if there is no currently active plan and this method has been called several times // Add a condition to prevent sending repeated requests (Note: This is a same-lane maneuver, so no need to consider lower bound of joining gap.) ++numLeaderAbortingCalls_; - ROS_DEBUG_STREAM("numLeaderAbortingCalls = " << numLeaderAbortingCalls_ << ", max = " << config_.maxLeaderAbortingCalls); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "numLeaderAbortingCalls = " << numLeaderAbortingCalls_ << ", max = " << config_.maxLeaderAbortingCalls); if (currentGap <= maxJoinGap && !pm_.current_plan.valid && numLeaderAbortingCalls_ > config_.maxLeaderAbortingCalls) { // compose frontal joining plan, senderID is the old leader - cav_msgs::MobilityRequest request; + carma_v2x_msgs::msg::MobilityRequest request; std::string planId = boost::uuids::to_string(boost::uuids::random_generator()()); - long currentTime = ros::Time::now().toNSec() / 1000000; + long currentTime = timer_factory_->now().nanoseconds() / 1000000; request.m_header.plan_id = planId; request.m_header.recipient_id = pm_.platoonLeaderID; //the new joiner request.m_header.sender_id = config_.vehicleID; @@ -2891,75 +2892,79 @@ namespace platoon_strategic_ihp request.strategy_params = fmter.str(); // assign a new plan type - request.plan_type.type = cav_msgs::PlanType::PLATOON_FRONT_JOIN; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_FRONT_JOIN; request.strategy = PLATOONING_STRATEGY; request.urgency = 50; request.location = pose_to_ecef(pose_msg_); mobility_request_publisher_(request); - ROS_WARN("Published Mobility Candidate-Join request to the new leader"); + RCLCPP_WARN(rclcpp::get_logger("platoon_strategic_ihp"),"Published Mobility Candidate-Join request to the new leader"); // Create a new join action plan pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.platoonLeaderID); } //Task 4: publish platoon status message - cav_msgs::MobilityOperation status; + carma_v2x_msgs::msg::MobilityOperation status; status = composeMobilityOperationLeaderAborting(); mobility_operation_publisher_(status); - ROS_DEBUG_STREAM("Published platoon STATUS operation message"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message"); - long tsEnd = ros::Time::now().toNSec() / 1000000; + long tsEnd = timer_factory_->now().nanoseconds() / 1000000; long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration / 1000).sleep(); + // TODO this solution is not sim-time complient and should be replaced with one which is when possible + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888 + std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration)); } // UCLA: frontal join state (inherited from leader waiting: prepare to join as th new leader) void PlatoonStrategicIHPPlugin::run_candidate_leader() { - ROS_DEBUG_STREAM("Run Candidate Leader State "); - long tsStart = ros::Time::now().toNSec() / 1000000; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Run Candidate Leader State "); + long tsStart = timer_factory_->now().nanoseconds() / 1000000; // Task 1: State time out if (tsStart - candidatestateStartTime > waitingStateTimeout * 1000) { //TODO if the current state timeouts, we need to have a kind of ABORT message to inform the applicant - ROS_DEBUG_STREAM("CandidateLeader state is timeout, changing back to PlatoonLeaderState."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "CandidateLeader state is timeout, changing back to PlatoonLeaderState."); pm_.current_platoon_state = PlatoonState::LEADER; pm_.clearActionPlan(); pm_.resetHostPlatoon(); } // Task 2: publish status message - cav_msgs::MobilityOperation status; + carma_v2x_msgs::msg::MobilityOperation status; status = composeMobilityOperationCandidateLeader(); mobility_operation_publisher_(status); - ROS_DEBUG_STREAM("publish status message"); - long tsEnd = ros::Time::now().toNSec() / 1000000; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "publish status message"); + long tsEnd = timer_factory_->now().nanoseconds() / 1000000; long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration / 1000).sleep(); + // TODO this solution is not sim-time complient and should be replaced with one which is when possible + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888 + std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration)); } // UCLA: add leading with operation state for cut-in join platoon leader void PlatoonStrategicIHPPlugin::run_lead_with_operation() { - long tsStart = ros::Time::now().toNSec() / 1000000; + long tsStart = timer_factory_->now().nanoseconds() / 1000000; // Task 1: heart beat timeout: constantly send INFO mob_op bool isTimeForHeartBeat = tsStart - prevHeartBeatTime_ >= infoMessageInterval_; - ROS_DEBUG_STREAM("time since last heart beat: " << tsStart - prevHeartBeatTime_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time since last heart beat: " << tsStart - prevHeartBeatTime_); if (isTimeForHeartBeat) { - cav_msgs::MobilityOperation infoOperation; + carma_v2x_msgs::msg::MobilityOperation infoOperation; infoOperation = composeMobilityOperationLeadWithOperation(OPERATION_INFO_TYPE); mobility_operation_publisher_(infoOperation); - prevHeartBeatTime_ = ros::Time::now().toNSec() / 1000000; + prevHeartBeatTime_ = timer_factory_->now().nanoseconds() / 1000000; } // // Task 3: plan time out // if (pm_.current_plan.valid) // { - // bool isCurrentPlanTimeout = ((ros::Time::now().toNSec() / 1000000 - pm_.current_plan.planStartTime) > NEGOTIATION_TIMEOUT); + // bool isCurrentPlanTimeout = ((timer_factory_->now().nanoseconds() / 1000000 - pm_.current_plan.planStartTime) > NEGOTIATION_TIMEOUT); // if (isCurrentPlanTimeout) // { - // ROS_DEBUG_STREAM("Give up waiting on plan with planId: " << pm_.current_plan.planId << "; stay in LEADWITHOPERATION"); + // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Give up waiting on plan with planId: " << pm_.current_plan.planId << "; stay in LEADWITHOPERATION"); // pm_.current_plan.valid = false; // } // } @@ -2969,13 +2974,15 @@ namespace platoon_strategic_ihp // if has follower, publish platoon message as STATUS mob_op if (hasFollower) { - cav_msgs::MobilityOperation statusOperation; + carma_v2x_msgs::msg::MobilityOperation statusOperation; statusOperation = composeMobilityOperationLeadWithOperation(OPERATION_STATUS_TYPE); mobility_operation_publisher_(statusOperation); } - long tsEnd = ros::Time::now().toNSec() / 1000000; + long tsEnd = timer_factory_->now().nanoseconds() / 1000000; long sleepDuration = std::max((int32_t)(statusMessageInterval_ - (tsEnd - tsStart)), 0); - ros::Duration(sleepDuration / 1000).sleep(); + // TODO this solution is not sim-time complient and should be replaced with one which is when possible + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1888 + std::this_thread::sleep_for(std::chrono::milliseconds(sleepDuration)); } // UCLA: add prepare to join state for cut-in joining vehicle @@ -2992,13 +2999,13 @@ namespace platoon_strategic_ihp */ // Task 2.1: state timeout - long tsStart = ros::Time::now().toNSec() / 1000000; + long tsStart = timer_factory_->now().nanoseconds() / 1000000; bool isCurrentStateTimeout = (tsStart - candidatestateStartTime) > waitingStateTimeout * 1000; - ROS_DEBUG_STREAM("timeout1: " << tsStart - candidatestateStartTime); - ROS_DEBUG_STREAM("waitingStateTimeout: " << waitingStateTimeout * 1000); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "timeout1: " << tsStart - candidatestateStartTime); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "waitingStateTimeout: " << waitingStateTimeout * 1000); if (isCurrentStateTimeout) { - ROS_DEBUG_STREAM("The current prepare to join state is timeout. Change back to leader state and abort lane change."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The current prepare to join state is timeout. Change back to leader state and abort lane change."); pm_.current_platoon_state = PlatoonState::LEADER; safeToLaneChange_ = false; pm_.clearActionPlan(); @@ -3009,31 +3016,31 @@ namespace platoon_strategic_ihp // TODO: Plan timeout is not needed for this state // If we aren't already waiting on a response to one of these plans, create one once neighbor info is available - ROS_DEBUG_STREAM("current_plan.valid = " << pm_.current_plan.valid << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_plan.valid = " << pm_.current_plan.valid << ", is_neighbor_record_complete = " << pm_.is_neighbor_record_complete_); if (!pm_.current_plan.valid && pm_.is_neighbor_record_complete_) { // Task 1: compose mobility operation (status) - cav_msgs::MobilityOperation status; + carma_v2x_msgs::msg::MobilityOperation status; status = composeMobilityOperationPrepareToJoin(); //TODO: I bet we could consolidate a lot of these compose methods mobility_operation_publisher_(status); - ROS_DEBUG_STREAM("Published platoon STATUS operation message"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published platoon STATUS operation message"); // Task 3: Calculate proper cut_in index // Note: The cut-in index is zero-based and points to the gap-leading vehicle's index. For cut-in from front, the join index = -1. double joinerDtD = current_downtrack_; target_join_index_ = pm_.getClosestIndex(joinerDtD); - ROS_DEBUG_STREAM("calculated join index: " << target_join_index_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "calculated join index: " << target_join_index_); // Task 4: Send out request to leader about cut-in position - cav_msgs::MobilityRequest request; + carma_v2x_msgs::msg::MobilityRequest request; std::string planId = boost::uuids::to_string(boost::uuids::random_generator()()); - long currentTime = ros::Time::now().toNSec() / 1000000; + long currentTime = timer_factory_->now().nanoseconds() / 1000000; request.m_header.plan_id = planId; request.m_header.recipient_id = pm_.neighbor_platoon_leader_id_; request.m_header.sender_id = config_.vehicleID; request.m_header.timestamp = currentTime; - request.plan_type.type = cav_msgs::PlanType::PLATOON_CUT_IN_JOIN; + request.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_CUT_IN_JOIN; request.strategy = PLATOONING_STRATEGY; request.urgency = 50; request.location = pose_to_ecef(pose_msg_); @@ -3048,7 +3055,7 @@ namespace platoon_strategic_ihp fmter %target_join_index_; // index = 5 request.strategy_params = fmter.str(); mobility_request_publisher_(request); - ROS_DEBUG_STREAM("Published Mobility cut-in join request to leader " << request.m_header.recipient_id << " with planId = " << planId); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Published Mobility cut-in join request to leader " << request.m_header.recipient_id << " with planId = " << planId); // Create a new join action plan pm_.current_plan = ActionPlan(true, currentTime, planId, pm_.neighbor_platoon_leader_id_); @@ -3063,9 +3070,7 @@ namespace platoon_strategic_ihp // Platoon on spin bool PlatoonStrategicIHPPlugin::onSpin() - { - plugin_discovery_publisher_(plugin_discovery_msg_); - + { // Update the platoon manager for host's current location & speeds pm_.updateHostPose(current_downtrack_, current_crosstrack_); pm_.updateHostSpeeds(current_speed_, cmd_speed_); @@ -3108,16 +3113,16 @@ namespace platoon_strategic_ihp } else if (pm_.current_platoon_state == PlatoonState::STANDBY) { - ROS_DEBUG_STREAM("standby state, nothing to do"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "standby state, nothing to do"); } // coding oversight else { - ROS_ERROR_STREAM("///// unhandled state " << pm_.current_platoon_state); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "///// unhandled state " << pm_.current_platoon_state); } // TODO: Place holder for departure - cav_msgs::PlatooningInfo platoon_status = composePlatoonInfoMsg(); + carma_planning_msgs::msg::PlatooningInfo platoon_status = composePlatoonInfoMsg(); platooning_info_publisher_(platoon_status); return true; @@ -3126,12 +3131,12 @@ namespace platoon_strategic_ihp // ------- Generate maneuver plan (Service Callback) ------- // // compose maneuver message - cav_msgs::Maneuver PlatoonStrategicIHPPlugin::composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, ros::Time& current_time) + carma_planning_msgs::msg::Maneuver PlatoonStrategicIHPPlugin::composeManeuverMessage(double current_dist, double end_dist, double current_speed, double target_speed, int lane_id, rclcpp::Time& current_time) { - cav_msgs::Maneuver maneuver_msg; - maneuver_msg.type = cav_msgs::Maneuver::LANE_FOLLOWING; - maneuver_msg.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::PLATOONING; - maneuver_msg.lane_following_maneuver.parameters.presence_vector = cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN; + carma_planning_msgs::msg::Maneuver maneuver_msg; + maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver_msg.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::PLATOONING; + maneuver_msg.lane_following_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN; maneuver_msg.lane_following_maneuver.parameters.planning_tactical_plugin = "platooning_tactical_plugin"; maneuver_msg.lane_following_maneuver.parameters.planning_strategic_plugin = "platoon_strategic_ihp"; maneuver_msg.lane_following_maneuver.start_dist = current_dist; @@ -3141,38 +3146,38 @@ namespace platoon_strategic_ihp maneuver_msg.lane_following_maneuver.end_speed = target_speed; // because it is a rough plan, assume vehicle can always reach to the target speed in a lanelet - maneuver_msg.lane_following_maneuver.end_time = current_time + ros::Duration(config_.time_step); + maneuver_msg.lane_following_maneuver.end_time = current_time + rclcpp::Duration(config_.time_step*1e9); maneuver_msg.lane_following_maneuver.lane_ids = { std::to_string(lane_id) }; - ROS_DEBUG_STREAM("in compose maneuver lane id:"<< lane_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "in compose maneuver lane id:"<< lane_id); lanelet::ConstLanelet current_lanelet = wm_->getMap()->laneletLayer.get(lane_id); if(!wm_->getMapRoutingGraph()->following(current_lanelet, false).empty()) { auto next_lanelet_id = wm_->getMapRoutingGraph()->following(current_lanelet, false).front().id(); - ROS_DEBUG_STREAM("next_lanelet_id:"<< next_lanelet_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "next_lanelet_id:"<< next_lanelet_id); maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(next_lanelet_id)); } else { - ROS_DEBUG_STREAM("No following lanelets"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "No following lanelets"); } current_time = maneuver_msg.lane_following_maneuver.end_time; - ROS_DEBUG_STREAM("Creating lane follow start dist:"<routeTrackPos(current_loc).downtrack; double speed_progress = current_speed_; - ros::Time time_progress = ros::Time::now(); + rclcpp::Time time_progress = timer_factory_->now(); // ---------------- use IHP platoon trajectory regulation here -------------------- // Note: The desired gap will be adjusted and send to control plugin (via platoon_info_msg) where gap creation will be handled. @@ -3281,32 +3286,32 @@ namespace platoon_strategic_ihp // Update current status based on prior plan if(req.prior_plan.maneuvers.size()!= 0) { - ROS_DEBUG_STREAM("Provided with initial plan..."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Provided with initial plan..."); time_progress = req.prior_plan.planning_completion_time; int end_lanelet = 0; updateCurrentStatus(req.prior_plan.maneuvers.back(), speed_progress, current_progress, end_lanelet); last_lanelet_index = findLaneletIndexFromPath(end_lanelet, shortest_path); } - ROS_DEBUG_STREAM("Starting Loop"); - ROS_DEBUG_STREAM("total_maneuver_length: " << total_maneuver_length << " route_length: " << route_length); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Starting Loop"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "total_maneuver_length: " << total_maneuver_length << " route_length: " << route_length); - ROS_DEBUG_STREAM("in mvr callback safeToLaneChange: " << safeToLaneChange_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "in mvr callback safeToLaneChange: " << safeToLaneChange_); // Note: Use current_lanlet list (which was determined based on vehicle pose) to find current lanelet ID. long current_lanelet_id = current_lanelets[0].second.id(); - ROS_DEBUG_STREAM("current_lanelet_id: " << current_lanelet_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_lanelet_id: " << current_lanelet_id); // lane change maneuver if (safeToLaneChange_) { // for testing purpose only, check lane change status double target_crosstrack = wm_->routeTrackPos(target_cutin_pose_).crosstrack; - ROS_DEBUG_STREAM("target_crosstrack: " << target_crosstrack); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_crosstrack: " << target_crosstrack); double crosstrackDiff = current_crosstrack_ - target_crosstrack; bool isLaneChangeFinished = abs(crosstrackDiff) <= config_.maxCrosstrackError; - ROS_DEBUG_STREAM("crosstrackDiff: " << crosstrackDiff); - ROS_DEBUG_STREAM("isLaneChangeFinished: " << isLaneChangeFinished); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "crosstrackDiff: " << crosstrackDiff); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "isLaneChangeFinished: " << isLaneChangeFinished); // lane change not finished, use lane change plan if(!isLaneChangeFinished) @@ -3314,37 +3319,37 @@ namespace platoon_strategic_ihp // send out lane change plan while (current_progress < total_maneuver_length) { - ROS_DEBUG_STREAM("Lane Change Maneuver for Cut-in join ! "); - ROS_DEBUG_STREAM("current_progress: "<< current_progress); - ROS_DEBUG_STREAM("speed_progress: " << speed_progress); - ROS_DEBUG_STREAM("target_speed: " << target_speed); - ROS_DEBUG_STREAM("time_progress: " << time_progress.toSec()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane Change Maneuver for Cut-in join ! "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_progress: "<< current_progress); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed_progress: " << speed_progress); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_speed: " << target_speed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time_progress: " << rclcpp::Time(time_progress).seconds()); // set to next lane destination, consider sending ecef instead of dtd double end_dist = total_maneuver_length; - ROS_DEBUG_STREAM("end_dist: " << end_dist); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "end_dist: " << end_dist); // consider calculate dtd_diff and ctd_diff double dist_diff = end_dist - current_progress; - ROS_DEBUG_STREAM("dist_diff: " << dist_diff); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dist_diff: " << dist_diff); //TODO: target_cutin_pose_ represents the platoon leader. It seems this may be the wrong answer for mid- or rear-cutins? //SAINA: currently, the functions do not provide the correct point of rear vehicle of the platoon double lc_end_dist = wm_->routeTrackPos(target_cutin_pose_).downtrack; - ROS_DEBUG_STREAM("lc_end_dist before buffer: " << lc_end_dist); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lc_end_dist before buffer: " << lc_end_dist); lc_end_dist = std::max(lc_end_dist, current_progress + config_.maxCutinGap); - ROS_DEBUG_STREAM("lc_end_dist after buffer: " << lc_end_dist); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lc_end_dist after buffer: " << lc_end_dist); //TODO: target_cutin_pose_ represents the platoon leader. Is this the best pose to use here? // get the actually closest lanelets, auto target_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, target_cutin_pose_, 1); if (target_lanelets.empty()) { - ROS_DEBUG_STREAM("The target cutin pose is not on a valid lanelet. So no lane change!"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The target cutin pose is not on a valid lanelet. So no lane change!"); break; } int target_lanelet_id = target_lanelets[0].second.id(); - ROS_DEBUG_STREAM("target_lanelet_id: " << target_lanelet_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_lanelet_id: " << target_lanelet_id); lanelet::ConstLanelet starting_lanelet = wm_->getMap()->laneletLayer.get(current_lanelet_id); lanelet::ConstLanelet ending_lanelet = wm_->getMap()->laneletLayer.get(target_lanelet_id); @@ -3364,14 +3369,14 @@ namespace platoon_strategic_ihp if (lanechangePossible) { - ROS_DEBUG_STREAM("Lane change possible, planning it.. " ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane change possible, planning it.. " ); resp.new_plan.maneuvers.push_back(composeLaneChangeManeuverMessage(current_downtrack_, lc_end_dist, speed_progress, target_speed, current_lanelet_id, target_lanelet_id , time_progress)); } else { - ROS_DEBUG_STREAM("Lane change impossible, planning lanefollow instead ... " ); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lane change impossible, planning lanefollow instead ... " ); resp.new_plan.maneuvers.push_back(composeManeuverMessage(current_downtrack_, end_dist, speed_progress, target_speed, current_lanelet_id, time_progress)); } @@ -3397,15 +3402,15 @@ namespace platoon_strategic_ihp // send out lane following plan while (current_progress < total_maneuver_length) { - ROS_DEBUG_STREAM("Same Lane Maneuver for platoon join ! "); - ROS_DEBUG_STREAM("current_progress: "<< current_progress); - ROS_DEBUG_STREAM("speed_progress: " << speed_progress); - ROS_DEBUG_STREAM("target_speed: " << target_speed); - ROS_DEBUG_STREAM("time_progress: " << time_progress.toSec()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Same Lane Maneuver for platoon join ! "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_progress: "<< current_progress); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed_progress: " << speed_progress); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_speed: " << target_speed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time_progress: " << rclcpp::Time(time_progress).seconds()); double end_dist = total_maneuver_length; - ROS_DEBUG_STREAM("end_dist: " << end_dist); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "end_dist: " << end_dist); double dist_diff = end_dist - current_progress; - ROS_DEBUG_STREAM("dist_diff: " << dist_diff); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dist_diff: " << dist_diff); if(end_dist < current_progress) { break; @@ -3430,18 +3435,18 @@ namespace platoon_strategic_ihp // same-lane maneuver else { - ROS_DEBUG_STREAM("Planning Same Lane Maneuver! "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Planning Same Lane Maneuver! "); while (current_progress < total_maneuver_length) { - ROS_DEBUG_STREAM("Same Lane Maneuver for platoon join ! "); - ROS_DEBUG_STREAM("current_progress: "<< current_progress); - ROS_DEBUG_STREAM("speed_progress: " << speed_progress); - ROS_DEBUG_STREAM("target_speed: " << target_speed); - ROS_DEBUG_STREAM("time_progress: " << time_progress.toSec()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Same Lane Maneuver for platoon join ! "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_progress: "<< current_progress); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "speed_progress: " << speed_progress); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_speed: " << target_speed); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "time_progress: " << rclcpp::Time(time_progress).seconds()); double end_dist = total_maneuver_length; - ROS_DEBUG_STREAM("end_dist: " << end_dist); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "end_dist: " << end_dist); double dist_diff = end_dist - current_progress; - ROS_DEBUG_STREAM("dist_diff: " << dist_diff); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "dist_diff: " << dist_diff); if (end_dist < current_progress) { break; @@ -3465,31 +3470,38 @@ namespace platoon_strategic_ihp if(resp.new_plan.maneuvers.size() == 0) { - ROS_WARN_STREAM("Cannot plan maneuver because no route is found"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Cannot plan maneuver because no route is found"); } if (pm_.getHostPlatoonSize() < 2 && !safeToLaneChange_) { resp.new_plan.maneuvers = {}; - ROS_WARN_STREAM("Platoon size 1 so Empty maneuver sent"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Platoon size 1 so Empty maneuver sent"); } else { - ROS_DEBUG_STREAM("Planning maneuvers: "); - ROS_DEBUG_STREAM("safeToLaneChange_: " << safeToLaneChange_); - ROS_DEBUG_STREAM("pm_.getHostPlatoonSize(): " << pm_.getHostPlatoonSize()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Planning maneuvers: "); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "safeToLaneChange_: " << safeToLaneChange_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "pm_.getHostPlatoonSize(): " << pm_.getHostPlatoonSize()); } if (pm_.current_platoon_state == PlatoonState::STANDBY) { pm_.current_platoon_state = PlatoonState::LEADER; - ROS_DEBUG_STREAM("change the state from standby to leader at start-up"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "change the state from standby to leader at start-up"); } - ROS_DEBUG_STREAM("current_downtrack: " << current_downtrack_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_downtrack: " << current_downtrack_); return true; } + //--------------------------------------------------------------------------// + + void PlatoonStrategicIHPPlugin::setConfig(const PlatoonPluginConfig& config) + { + config_ = config; + } + } diff --git a/platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp b/platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp new file mode 100644 index 0000000000..dd95638750 --- /dev/null +++ b/platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp @@ -0,0 +1,241 @@ +/* + * 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. + */ +#include "platoon_strategic_ihp/platoon_strategic_plugin_node_ihp.h" +#include +#include + +namespace platoon_strategic_ihp +{ + namespace std_ph = std::placeholders; + + Node::Node(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::StrategicPlugin(options) + { + // Create initial config + config_ = PlatoonPluginConfig(); + + // Declare parameters + + declare_parameter("allowCutinJoin", config_.allowCutinJoin); + declare_parameter("test_cutin_join", config_.test_cutin_join); + declare_parameter("test_front_join", config_.test_front_join); + + declare_parameter("maxPlatoonSize", config_.maxPlatoonSize); + declare_parameter("algorithmType", config_.algorithmType); + declare_parameter("statusMessageInterval", config_.statusMessageInterval); + declare_parameter("infoMessageInterval", config_.infoMessageInterval); + declare_parameter("join_index", config_.join_index); + + declare_parameter("timeHeadway", config_.timeHeadway); + declare_parameter("standStillHeadway", config_.standStillHeadway); + declare_parameter("maxAllowedJoinTimeGap", config_.maxAllowedJoinTimeGap); + declare_parameter("maxAllowedJoinGap", config_.maxAllowedJoinGap); + declare_parameter("minAllowedJoinGap", config_.minAllowedJoinGap); + declare_parameter("desiredJoinTimeGap", config_.desiredJoinTimeGap); + declare_parameter("desiredJoinGap", config_.desiredJoinGap); + declare_parameter("waitingStateTimeout", config_.waitingStateTimeout); + declare_parameter("cmdSpeedMaxAdjustment", config_.cmdSpeedMaxAdjustment); + declare_parameter("minAllowableHeadaway", config_.minAllowableHeadaway); + declare_parameter("maxAllowableHeadaway", config_.maxAllowableHeadaway); + declare_parameter("headawayStableLowerBond", config_.headawayStableLowerBond); + declare_parameter("headawayStableUpperBond", config_.headawayStableUpperBond); + declare_parameter("minCutinGap", config_.minCutinGap); + declare_parameter("maxCutinGap", config_.maxCutinGap); + declare_parameter("maxCrosstrackError", config_.maxCrosstrackError); + declare_parameter("minPlatooningSpeed", config_.minPlatooningSpeed); + declare_parameter("significantDTDchange", config_.significantDTDchange); + declare_parameter("longitudinalCheckThresold", config_.longitudinalCheckThresold); + declare_parameter("vehicle_length", config_.vehicleLength); + declare_parameter("vehicle_id", config_.vehicleID); + } + + rcl_interfaces::msg::SetParametersResult Node::parameter_update_callback(const std::vector ¶meters) + { + + auto error = update_params({ + {"test_front_join", config_.test_front_join}, + {"allowCutinJoin", config_.allowCutinJoin}, + {"test_cutin_join", config_.test_cutin_join} + }, parameters); + + auto error2 = update_params({ + {"maxPlatoonSize", config_.maxPlatoonSize}, + {"algorithmType", config_.algorithmType}, + {"statusMessageInterval", config_.statusMessageInterval}, + {"infoMessageInterval", config_.infoMessageInterval}, + {"join_index", config_.join_index} + }, parameters); + + auto error3 = update_params({ + {"timeHeadway", config_.timeHeadway}, + {"standStillHeadway", config_.standStillHeadway}, + {"maxAllowedJoinTimeGap", config_.maxAllowedJoinTimeGap}, + {"maxAllowedJoinGap", config_.maxAllowedJoinGap}, + {"minAllowedJoinGap", config_.minAllowedJoinGap}, + {"desiredJoinTimeGap", config_.desiredJoinTimeGap}, + {"desiredJoinGap", config_.desiredJoinGap}, + {"waitingStateTimeout", config_.waitingStateTimeout}, + {"cmdSpeedMaxAdjustment", config_.cmdSpeedMaxAdjustment}, + {"minAllowableHeadaway", config_.minAllowableHeadaway}, + {"maxAllowableHeadaway", config_.maxAllowableHeadaway}, + {"headawayStableLowerBond", config_.headawayStableLowerBond}, + {"headawayStableUpperBond", config_.headawayStableUpperBond}, + {"minCutinGap", config_.minCutinGap}, + {"maxCutinGap", config_.maxCutinGap}, + {"maxCrosstrackError", config_.maxCrosstrackError}, + {"minPlatooningSpeed", config_.minPlatooningSpeed}, + {"significantDTDchange", config_.significantDTDchange}, + {"longitudinalCheckThresold", config_.longitudinalCheckThresold} + // vehicle_length and id excluded since they should not be updated at runtime + }, parameters); + + if (worker_) + { + worker_->setConfig(config_); + } + + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error && !error2 && !error3; + + return result; + } + + carma_ros2_utils::CallbackReturn Node::on_configure_plugin() + { + // Reset config + config_ = PlatoonPluginConfig(); + + // Load parameters + get_parameter("test_front_join", config_.test_front_join); + get_parameter("allowCutinJoin", config_.allowCutinJoin); + get_parameter("test_cutin_join", config_.test_cutin_join); + + get_parameter("maxPlatoonSize", config_.maxPlatoonSize); + get_parameter("algorithmType", config_.algorithmType); + get_parameter("statusMessageInterval", config_.statusMessageInterval); + get_parameter("infoMessageInterval", config_.infoMessageInterval); + get_parameter("join_index", config_.join_index); + + get_parameter("timeHeadway", config_.timeHeadway); + get_parameter("standStillHeadway", config_.standStillHeadway); + get_parameter("maxAllowedJoinTimeGap", config_.maxAllowedJoinTimeGap); + get_parameter("maxAllowedJoinGap", config_.maxAllowedJoinGap); + get_parameter("minAllowedJoinGap", config_.minAllowedJoinGap); + get_parameter("desiredJoinTimeGap", config_.desiredJoinTimeGap); + get_parameter("desiredJoinGap", config_.desiredJoinGap); + get_parameter("waitingStateTimeout", config_.waitingStateTimeout); + get_parameter("cmdSpeedMaxAdjustment", config_.cmdSpeedMaxAdjustment); + get_parameter("minAllowableHeadaway", config_.minAllowableHeadaway); + get_parameter("maxAllowableHeadaway", config_.maxAllowableHeadaway); + get_parameter("headawayStableLowerBond", config_.headawayStableLowerBond); + get_parameter("headawayStableUpperBond", config_.headawayStableUpperBond); + get_parameter("minCutinGap", config_.minCutinGap); + get_parameter("maxCutinGap", config_.maxCutinGap); + get_parameter("maxCrosstrackError", config_.maxCrosstrackError); + get_parameter("minPlatooningSpeed", config_.minPlatooningSpeed); + get_parameter("significantDTDchange", config_.significantDTDchange); + get_parameter("longitudinalCheckThresold", config_.longitudinalCheckThresold); + get_parameter("vehicle_length", config_.vehicleLength); + get_parameter("vehicle_id", config_.vehicleID); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1)); + + // Setup publishers + mob_response_pub = create_publisher("outgoing_mobility_response", 5); + mob_request_pub = create_publisher("outgoing_mobility_request", 5); + mob_operation_pub = create_publisher("outgoing_mobility_operation", 5); + platoon_info_pub = create_publisher("platoon_info", 1); + + // Build worker + + worker_ = std::make_shared(get_world_model(), config_, [this](auto msg) { this->mob_response_pub->publish(msg); }, + [this](auto msg) { this->mob_request_pub->publish(msg); }, [this](auto msg) { this->mob_operation_pub->publish(msg); }, + [this](auto msg) { this->platoon_info_pub->publish(msg); }, + std::make_shared(shared_from_this())); + + + // Setup subscribers + mob_request_sub = create_subscription("incoming_mobility_request", 10, + std::bind(&PlatoonStrategicIHPPlugin::mob_req_cb, worker_.get(), std_ph::_1)); + + mob_response_sub = create_subscription("incoming_mobility_response", 10, + std::bind(&PlatoonStrategicIHPPlugin::mob_resp_cb, worker_.get(), std_ph::_1)); + + mob_operation_sub = create_subscription("incoming_mobility_operation", 10, + std::bind(&PlatoonStrategicIHPPlugin::mob_op_cb, worker_.get(), std_ph::_1)); + + current_pose_sub = create_subscription("current_pose", 10, + std::bind(&PlatoonStrategicIHPPlugin::pose_cb, worker_.get(), std_ph::_1)); + + current_twist_sub = create_subscription("current_velocity", 10, + std::bind(&PlatoonStrategicIHPPlugin::twist_cb, worker_.get(), std_ph::_1)); + + cmd_sub = create_subscription("twist_raw", 10, + std::bind(&PlatoonStrategicIHPPlugin::cmd_cb, worker_.get(), std_ph::_1)); + + georeference_sub = create_subscription("georeference", 10, + std::bind(&PlatoonStrategicIHPPlugin::georeference_cb, worker_.get(), std_ph::_1)); + + loop_timer_ = create_timer( + get_clock(), + std::chrono::milliseconds(100), // 10 Hz frequency + std::bind(&PlatoonStrategicIHPPlugin::onSpin, worker_.get())); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + carma_ros2_utils::CallbackReturn Node::on_cleanup_plugin() + { + // Ensure subscribers are disconnected incase cleanup is called, we don't want to keep driving the worker + mob_response_sub.reset(); + mob_operation_sub.reset(); + current_pose_sub.reset(); + current_twist_sub.reset(); + cmd_sub.reset(); + georeference_sub.reset(); + worker_.reset(); + + return CallbackReturn::SUCCESS; + } + + + void Node::plan_maneuvers_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) + { + if (worker_) + worker_->plan_maneuver_cb(*req, *resp); + } + + bool Node::get_availability() { + return true; + } + + std::string Node::get_version_id() { + return "v4.0"; + } + +} // platoon_strategic_ihp + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(platoon_strategic_ihp::Node) diff --git a/platooning_strategic_IHP/test/main_test.cpp b/platooning_strategic_IHP/test/main_test.cpp index 81d202fa20..a1dcddca03 100644 --- a/platooning_strategic_IHP/test/main_test.cpp +++ b/platooning_strategic_IHP/test/main_test.cpp @@ -1,27 +1,38 @@ - -/*------------------------------------------------------------------------------ -* Copyright (C) 2020-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. - -------------------------------------------------------------------------------*/ +/* + * 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. + */ #include -#include +#include +#include +#include +#include -// Run all the tests -int main(int argc, char **argv) +#include "platoon_strategic_ihp/platoon_strategic_plugin_node_ihp.h" + +int main(int argc, char ** argv) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} + ::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/platooning_strategic_IHP/test/mobility_messages.cpp b/platooning_strategic_IHP/test/mobility_messages.cpp index d1457182e1..05054e9d32 100644 --- a/platooning_strategic_IHP/test/mobility_messages.cpp +++ b/platooning_strategic_IHP/test/mobility_messages.cpp @@ -16,38 +16,38 @@ ------------------------------------------------------------------------------*/ -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include using namespace std; class MobilityMessages{ private: - cav_msgs::MobilityRequest req1; - cav_msgs::MobilityRequest req2; - cav_msgs::MobilityRequest req3; + carma_v2x_msgs::msg::MobilityRequest req1; + carma_v2x_msgs::msg::MobilityRequest req2; + carma_v2x_msgs::msg::MobilityRequest req3; // UCLA add req4 to test added PlanType - cav_msgs::MobilityRequest req4; - cav_msgs::MobilityResponse res1; - cav_msgs::MobilityResponse res2; - cav_msgs::MobilityResponse res3; - cav_msgs::MobilityOperation op1; - cav_msgs::MobilityOperation op2; - cav_msgs::MobilityOperation op3; + carma_v2x_msgs::msg::MobilityRequest req4; + carma_v2x_msgs::msg::MobilityResponse res1; + carma_v2x_msgs::msg::MobilityResponse res2; + carma_v2x_msgs::msg::MobilityResponse res3; + carma_v2x_msgs::msg::MobilityOperation op1; + carma_v2x_msgs::msg::MobilityOperation op2; + carma_v2x_msgs::msg::MobilityOperation op3; public: MobilityMessages(){ - cav_msgs::MobilityHeader header; - cav_msgs::PlanType plan; - cav_msgs::LocationECEF location; - cav_msgs::LocationOffsetECEF offset; - cav_msgs::Trajectory traject; - vector offsets; + carma_v2x_msgs::msg::MobilityHeader header; + carma_v2x_msgs::msg::PlanType plan; + carma_v2x_msgs::msg::LocationECEF location; + carma_v2x_msgs::msg::LocationOffsetECEF offset; + carma_v2x_msgs::msg::Trajectory traject; + vector offsets; //mobility request messages //set up location @@ -76,7 +76,7 @@ class MobilityMessages{ header.timestamp = 55555555555555; //set up mobility request - req1.header = header; + req1.m_header = header; req1.strategy = "CARMA/platooning"; req1.plan_type = plan; req1.urgency = 1000; @@ -112,9 +112,9 @@ class MobilityMessages{ header.timestamp = 555555555555; //set up mobility request - req2.header = header; + req2.m_header = header; req2.strategy = "CARMA/platooning"; - req2.plan_type.type = cav_msgs::PlanType::PLATOON_FOLLOWER_JOIN; + req2.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_FOLLOWER_JOIN; req2.urgency = 1000; req2.location = location; req2.strategy_params = "param1 param2 param3 param4"; @@ -148,7 +148,7 @@ class MobilityMessages{ header.timestamp = 98745745; //set up mobility request - req3.header = header; + req3.m_header = header; req3.strategy = "CARMA/platooning3"; req3.plan_type = plan; req3.urgency = 156; @@ -185,9 +185,9 @@ class MobilityMessages{ header.timestamp = 2222222222222; //set up mobility request - req4.header = header; + req4.m_header = header; req4.strategy = "CARMA/platooning"; - req4.plan_type.type = cav_msgs::PlanType::PLATOON_FRONT_JOIN; + req4.plan_type.type = carma_v2x_msgs::msg::PlanType::PLATOON_FRONT_JOIN; req4.urgency = 1300; req4.location = location; req4.strategy_params = "param1 param2 param3 param4"; @@ -206,7 +206,7 @@ class MobilityMessages{ header.timestamp = 66666666; //set up mobility response - res1.header = header; + res1.m_header = header; res1.is_accepted = true; res1.urgency = 1000; @@ -218,7 +218,7 @@ class MobilityMessages{ header.timestamp = 1772327; //set up mobility response - res2.header = header; + res2.m_header = header; res2.is_accepted = false; res2.urgency = 50; @@ -230,7 +230,7 @@ class MobilityMessages{ header.timestamp = 77777777; //set up mobility response - res3.header = header; + res3.m_header = header; res3.is_accepted = true; res3.urgency = 700; @@ -246,7 +246,7 @@ class MobilityMessages{ header.plan_id = "plan_id3"; header.timestamp = 44444444; - op1.header = header; + op1.m_header = header; op1.strategy = "STATUS"; op1.strategy_params = "CMDSPEED:1.0,DTD:1.0,SPEED:1.0"; @@ -257,7 +257,7 @@ class MobilityMessages{ header.plan_id = "plan_id12"; header.timestamp = 8989898; - op2.header = header; + op2.m_header = header; op2.strategy = "strategy3"; op2.strategy_params = "param1 param2 param3 param4 param5"; @@ -269,46 +269,46 @@ class MobilityMessages{ header.plan_id = "plan_id23"; header.timestamp = 181818; - op3.header = header; + op3.m_header = header; op3.strategy = "strategy23"; op3.strategy_params = ""; } - cav_msgs::MobilityRequest getRequest1(){ + carma_v2x_msgs::msg::MobilityRequest getRequest1(){ return req1; } - cav_msgs::MobilityRequest getRequest2(){ + carma_v2x_msgs::msg::MobilityRequest getRequest2(){ return req2; } - cav_msgs::MobilityRequest getRequest3(){ + carma_v2x_msgs::msg::MobilityRequest getRequest3(){ return req3; } - cav_msgs::MobilityResponse getResponse1(){ + carma_v2x_msgs::msg::MobilityResponse getResponse1(){ return res1; } - cav_msgs::MobilityResponse getResponse2(){ + carma_v2x_msgs::msg::MobilityResponse getResponse2(){ return res2; } - cav_msgs::MobilityResponse getResponse3(){ + carma_v2x_msgs::msg::MobilityResponse getResponse3(){ return res3; } - cav_msgs::MobilityOperation getOperation1(){ + carma_v2x_msgs::msg::MobilityOperation getOperation1(){ return op1; } - cav_msgs::MobilityOperation getOperation2(){ + carma_v2x_msgs::msg::MobilityOperation getOperation2(){ return op2; } - cav_msgs::MobilityOperation getOperation3(){ + carma_v2x_msgs::msg::MobilityOperation getOperation3(){ return op3; } diff --git a/platooning_strategic_IHP/test/test_platoon_manager.cpp b/platooning_strategic_IHP/test/test_platoon_manager.cpp index 8e87ea8261..a5e98ef61d 100644 --- a/platooning_strategic_IHP/test/test_platoon_manager.cpp +++ b/platooning_strategic_IHP/test/test_platoon_manager.cpp @@ -16,16 +16,14 @@ ------------------------------------------------------------------------------*/ -#include "platoon_manager_ihp.h" -#include "platoon_strategic_ihp.h" -#include "platoon_config_ihp.h" +#include "platoon_strategic_ihp/platoon_manager_ihp.h" +#include "platoon_strategic_ihp/platoon_strategic_ihp.h" +#include "platoon_strategic_ihp/platoon_config_ihp.h" #include -#include -#include -#include -#include -#include -// #include "TestHelpers.h" +#include +#include +#include +#include using namespace platoon_strategic_ihp; @@ -34,7 +32,8 @@ TEST(PlatoonManagerTest, test_construct) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use Getter to retrieve host Platoon Manager class PlatoonManager pm_ = plugin.getHostPM(); pm_.current_platoon_state = PlatoonState::LEADER; @@ -42,14 +41,13 @@ TEST(PlatoonManagerTest, test_construct) TEST(PlatoonManagerTest, test_ecef_encode) { - ros::Time::init(); - PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); - cav_msgs::LocationECEF ecef_point_test; + carma_v2x_msgs::msg::LocationECEF ecef_point_test; ecef_point_test.ecef_x = 1.0; ecef_point_test.ecef_y = 2.0; ecef_point_test.ecef_z = 3.0; @@ -61,7 +59,7 @@ TEST(PlatoonManagerTest, test_ecef_encode) TEST(PlatoonManagerTest, test_split) { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; std::string strategyParams("INFO|REAR:1,LENGTH:2,SPEED:3,SIZE:4"); std::vector inputsParams; boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); @@ -79,16 +77,14 @@ TEST(PlatoonManagerTest, test_split) // TEST(PlatoonManagerTest, test_states) // { -// ros::Time::init(); - // PlatoonPluginConfig config; // std::shared_ptr wm = std::make_shared(); -// PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); +// PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}); // pm_.current_platoon_state = PlatoonState::LEADER; // pm_.current_downtrack_distance_ = 20; -// cav_msgs::MobilityRequest request; +// carma_v2x_msgs::msg::MobilityRequest request; // request.plan_type.type = 3; // request.strategy_params = "SIZE:1,SPEED:0,DTD:11.5599"; @@ -118,7 +114,8 @@ TEST(PlatoonStrategicIHPPlugin, mob_resp_cb) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use Getter to retrieve host Platoon Manager class PlatoonManager pm_ = plugin.getHostPM(); pm_.current_platoon_state = PlatoonState::FOLLOWER; @@ -132,22 +129,23 @@ TEST(PlatoonStrategicIHPPlugin, platoon_info_pub) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use Getter to retrieve host Platoon Manager class PlatoonManager pm_ = plugin.getHostPM(); pm_.current_platoon_state = PlatoonState::LEADER; - cav_msgs::PlatooningInfo info_msg1 = plugin.composePlatoonInfoMsg(); + carma_planning_msgs::msg::PlatooningInfo info_msg1 = plugin.composePlatoonInfoMsg(); EXPECT_EQ(info_msg1.leader_id, "default_id"); pm_.current_platoon_state = PlatoonState::FOLLOWER; pm_.isFollower = true; - PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); std::vector cur_pl; cur_pl.push_back(member); - pm_.platoon = cur_pl; + pm_.host_platoon_ = cur_pl; - cav_msgs::PlatooningInfo info_msg2 = plugin.composePlatoonInfoMsg(); + carma_planning_msgs::msg::PlatooningInfo info_msg2 = plugin.composePlatoonInfoMsg(); EXPECT_EQ(info_msg2.leader_id, "1"); } @@ -156,17 +154,18 @@ TEST(PlatoonStrategicIHPPlugin, test_follower) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use Getter to retrieve host Platoon Manager class PlatoonManager pm_ = plugin.getHostPM(); pm_.current_platoon_state = PlatoonState::CANDIDATEFOLLOWER; pm_.current_plan.valid = true; EXPECT_EQ(pm_.isFollower, false); - cav_msgs::MobilityResponse resp; - resp.header.plan_id = "resp"; - resp.is_accepted = true; - plugin.mob_resp_cb(resp); + auto resp = std::make_unique(); + resp->m_header.plan_id = "resp"; + resp->is_accepted = true; + plugin.mob_resp_cb(std::move(resp)); EXPECT_EQ(pm_.current_platoon_state, PlatoonState::FOLLOWER); EXPECT_EQ(pm_.isFollower, true); } @@ -176,20 +175,21 @@ TEST(PlatoonStrategicIHPPlugin, test_get_leader) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use Getter to retrieve host Platoon Manager class PlatoonManager pm_ = plugin.getHostPM(); pm_.current_platoon_state = PlatoonState::FOLLOWER; - PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); std::vector cur_pl; cur_pl.push_back(member); - pm_.platoon = cur_pl; + pm_.host_platoon_ = cur_pl; - EXPECT_EQ(pm_.platoon.size(), 1); + EXPECT_EQ(pm_.host_platoon_.size(), 1ul); - PlatoonMember member1 = pm_.platoon[0]; + PlatoonMember member1 = pm_.host_platoon_[0]; pm_.isFollower = true; PlatoonMember platoon_leader = pm_.getDynamicLeader(); @@ -201,74 +201,65 @@ TEST(PlatoonStrategicIHPPlugin, test_get_leader) TEST(PlatoonManagerTest, test2) { - platoon_strategic_ihp::PlatoonMember* member = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); + platoon_strategic_ihp::PlatoonMember* member = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); std::vector cur_pl; cur_pl.push_back(*member); - platoon_strategic_ihp::PlatoonManager pm; - pm.platoon = cur_pl; + platoon_strategic_ihp::PlatoonManager pm(std::make_shared()); + pm.host_platoon_ = cur_pl; pm.isFollower = true; - pm.platoonSize = 1; - pm.leaderID = "0"; + pm.platoonLeaderID = "0"; pm.currentPlatoonID = "a"; std::string params = "CMDSPEED:11,DOWNTRACK:01,SPEED:11"; - ros::Time::init(); - - pm.updatesOrAddMemberInfo("2", 2.0, 1.0, 2.5); + pm.updatesOrAddMemberInfo(cur_pl, "2", 2.0, 1.0, 0.0, 2.5); //HERE - EXPECT_EQ(2, pm.platoon.size()); - EXPECT_EQ("1", pm.platoon[0].staticId); + EXPECT_EQ(2ul, cur_pl.size()); + EXPECT_EQ("2", cur_pl[0].staticId); } TEST(PlatoonManagerTest, test3) { - platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); - platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 0, 200); std::vector cur_pl; cur_pl.push_back(*member1); cur_pl.push_back(*member2); - platoon_strategic_ihp::PlatoonManager pm; - pm.platoon = cur_pl; + platoon_strategic_ihp::PlatoonManager pm(std::make_shared()); + pm.host_platoon_ = cur_pl; pm.isFollower = false; - pm.platoonSize = 2; - pm.leaderID = "0"; + pm.platoonLeaderID = "0"; pm.currentPlatoonID = "a"; - ros::Time::init(); + int res = pm.getHostPlatoonSize(); - int res = pm.getTotalPlatooningSize(); - - EXPECT_EQ(3, res); + EXPECT_EQ(2, res); } TEST(PlatoonManagerTest, test4) { - platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); - platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 0, 200); std::vector cur_pl; cur_pl.push_back(*member1); cur_pl.push_back(*member2); - platoon_strategic_ihp::PlatoonManager pm; - pm.platoon = cur_pl; + platoon_strategic_ihp::PlatoonManager pm(std::make_shared()); + pm.host_platoon_ = cur_pl; pm.isFollower = true; - pm.platoonSize = 2; - pm.leaderID = "0"; + pm.platoonLeaderID = "0"; pm.currentPlatoonID = "a"; - ros::Time::init(); - int res = pm.allPredecessorFollowing(); EXPECT_EQ(0, res); diff --git a/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp b/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp index c5e6ba1471..7b84499877 100644 --- a/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp +++ b/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp @@ -23,29 +23,29 @@ * Author: Xu Han, Xin Xia, Jiaqi Ma */ -#include "platoon_manager_ihp.h" -#include "platoon_strategic_ihp.h" -#include "platoon_config_ihp.h" +#include "platoon_strategic_ihp/platoon_manager_ihp.h" +#include "platoon_strategic_ihp/platoon_strategic_ihp.h" +#include "platoon_strategic_ihp/platoon_config_ihp.h" #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -// #include "TestHelpers.h" -using namespace platoon_strategic_ihp; +namespace platoon_strategic_ihp +{ TEST(PlatoonManagerTestFrontJoin, test_construct_front) { - ros::Time::init(); - PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use Getter to retrieve host Platoon Manager class // change state plugin.setPMState(PlatoonState::LEADER); @@ -54,13 +54,12 @@ TEST(PlatoonManagerTestFrontJoin, test_construct_front) // UCLA: use "run_candidate_leader" to test ecef encoding TEST(PlatoonManagerTestFrontJoin, test_ecef_encode_front) { - ros::Time::init(); - PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); - cav_msgs::LocationECEF ecef_point_test; + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); + carma_v2x_msgs::msg::LocationECEF ecef_point_test; ecef_point_test.ecef_x = 1.0; ecef_point_test.ecef_y = 2.0; ecef_point_test.ecef_z = 3.0; @@ -72,7 +71,7 @@ TEST(PlatoonManagerTestFrontJoin, test_ecef_encode_front) TEST(PlatoonManagerTestFrontJoin, test_split_front) { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; std::string strategyParams("INFO|REAR:1,LENGTH:2,SPEED:3,SIZE:4"); std::vector inputsParams; boost::algorithm::split(inputsParams, strategyParams, boost::is_any_of(",")); @@ -106,10 +105,10 @@ TEST(PlatoonManagerTestFrontJoin, test_compose_front) TEST(PlatoonStrategicIHPPlugin, mob_resp_cb_front) { - PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); + PlatoonPluginConfig config;; - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(carma_wm::test::getGuidanceTestMap(), config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // change state plugin.setPMState(PlatoonState::FOLLOWER); @@ -119,33 +118,34 @@ TEST(PlatoonStrategicIHPPlugin, mob_resp_cb_front) TEST(PlatoonStrategicIHPPlugin, platoon_info_pub_front) { PlatoonPluginConfig config; - std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(carma_wm::test::getGuidanceTestMap(), config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use platoon manager setter to set state plugin.setPMState(PlatoonState::LEADER); // compose platoon info - cav_msgs::PlatooningInfo info_msg1 = plugin.composePlatoonInfoMsg(); + carma_planning_msgs::msg::PlatooningInfo info_msg1 = plugin.composePlatoonInfoMsg(); // check platoon info (Host is a single ADS vehicle, so it's also the leader) EXPECT_EQ(info_msg1.leader_id, "default_id"); // set host to follower plugin.setPMState(PlatoonState::FOLLOWER); - // set plan valid - plugin.setPMValid(true); // add member - PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); std::vector cur_pl; cur_pl.push_back(member); // update platoon list plugin.updatePlatoonList(cur_pl); + + plugin.pm_.changeFromLeaderToFollower("a", "1"); // compose platoon info - cav_msgs::PlatooningInfo info_msg2 = plugin.composePlatoonInfoMsg(); + carma_planning_msgs::msg::PlatooningInfo info_msg2 = plugin.composePlatoonInfoMsg(); // check platoo info (when host is follower, the newly added member will be the leader) EXPECT_EQ(info_msg2.leader_id, "1"); } + // ---------------------------------------- UCLA Tests for same-lane front join and cut-in front join --------------------------------------- // UCLA: Use the transition "leader_aborting --> follower" to test follower functions TEST(PlatoonStrategicIHPPlugin, test_follower_front) @@ -154,22 +154,22 @@ TEST(PlatoonStrategicIHPPlugin, test_follower_front) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // change state plugin.setPMState(PlatoonState::LEADERABORTING); - // set plan valid - plugin.setPMValid(true); // Use Getter to retrieve host Platoon Manager class platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); EXPECT_EQ(pm_.isFollower, false); - cav_msgs::MobilityResponse resp; - resp.is_accepted = true; + auto resp = std::make_unique(); + + resp->is_accepted = true; // sender id need to match with default value - resp.header.sender_id = pm_.targetLeaderId; + resp->m_header.sender_id = pm_.neighbor_platoon_leader_id_; // plan_id need to match with default value, which is "" - resp.header.plan_id = pm_.current_plan.planId; - plugin.mob_resp_cb(resp); + resp->m_header.plan_id = pm_.current_plan.planId; + plugin.mob_resp_cb(std::move(resp)); // Use Getter to retrieve an updated Platoon Manager class platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); @@ -183,20 +183,19 @@ TEST(PlatoonStrategicIHPPlugin, cutin_test_leader_2) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // change state plugin.setPMState(PlatoonState::LEADWITHOPERATION); - // set plan valid - plugin.setPMValid(true); // Use Getter to retrieve host Platoon Manager class platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); EXPECT_EQ(pm_.isFollower, false); // compose dummy request msg - cav_msgs::MobilityRequest msg; - msg.header.plan_id = "req"; - msg.plan_type.type = cav_msgs::PlanType::CUT_IN_FRONT_DONE; + carma_v2x_msgs::msg::MobilityRequest msg; + msg.m_header.plan_id = "req"; + msg.plan_type.type = carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE; // compose dummy response msg MobilityRequestResponse resp = plugin.handle_mob_req(msg); @@ -209,32 +208,31 @@ TEST(PlatoonStrategicIHPPlugin, cutin_test_leader_2) EXPECT_EQ(resp, MobilityRequestResponse::ACK); } -//UCLA: Cut-in front test for joining vheicle. Test the transition "leader --> prepare to jion" +//UCLA: Cut-in front test for joining vehicle. Test the transition "leader --> prepare to join" TEST(PlatoonStrategicIHPPlugin, cutin_test_join_1) { - ros::Time::init(); PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // Use Getter to retrieve host Platoon Manager class platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); plugin.setPMState(PlatoonState::LEADER); - plugin.setPMValid(true); EXPECT_EQ(pm_.isFollower, false); // compose dummy response msg - cav_msgs::MobilityResponse resp; + auto resp = std::make_unique(); // compose plan ID and leader ID - resp.header.plan_id = pm_.current_plan.planId; - resp.header.sender_id = pm_.current_plan.peerId; + resp->m_header.plan_id = pm_.current_plan.planId; + resp->m_header.sender_id = pm_.current_plan.peerId; // plan type and acceptance - resp.plan_type.type = cav_msgs::PlanType::CUT_IN_FROM_SIDE; - resp.is_accepted = true; + resp->plan_type.type = carma_v2x_msgs::msg::PlanType::CUT_IN_FROM_SIDE; + resp->is_accepted = true; // send dummy reponse - plugin.mob_resp_cb(resp); + plugin.mob_resp_cb(std::move(resp)); // Use Getter to retrieve host Platoon Manager class platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); @@ -242,32 +240,31 @@ TEST(PlatoonStrategicIHPPlugin, cutin_test_join_1) } -//UCLA: Cut-in front test for joining vheicle. Test the transition "prepare to jion --> candidate leader" +//UCLA: Cut-in front test for joining vehicle. Test the transition "prepare to join --> candidate leader" TEST(PlatoonStrategicIHPPlugin, cutin_test_join_2) { PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // change state plugin.setPMState(PlatoonState::PREPARETOJOIN); - // set plan valid - plugin.setPMValid(true); // Use Getter to retrieve host Platoon Manager class platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); EXPECT_EQ(pm_.isFollower, false); // compose dummy response msg - cav_msgs::MobilityResponse resp; + auto resp = std::make_unique(); // sender id need to match with default value - resp.header.sender_id = pm_.targetLeaderId; + resp->m_header.sender_id = pm_.neighbor_platoon_leader_id_; // plan_id need to match with default value, which is "" - resp.header.plan_id = pm_.current_plan.planId; - resp.is_accepted = true; - resp.plan_type.type = cav_msgs::PlanType::CUT_IN_FRONT_DONE; + resp->m_header.plan_id = pm_.current_plan.planId; + resp->is_accepted = true; + resp->plan_type.type = carma_v2x_msgs::msg::PlanType::CUT_IN_FRONT_DONE; // send dummy reponse - plugin.mob_resp_cb(resp); + plugin.mob_resp_cb(std::move(resp)); // run test and compare result // Use Getter to update Platoon Manager class @@ -283,11 +280,12 @@ TEST(PlatoonStrategicIHPPlugin, test_get_leader_front) PlatoonPluginConfig config; std::shared_ptr wm = std::make_shared(); - PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}, [&](auto msg) {}); + PlatoonStrategicIHPPlugin plugin(wm, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); // change state plugin.setPMState(PlatoonState::FOLLOWER); - PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 100); + PlatoonMember member = PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); std::vector cur_pl; cur_pl.push_back(member); @@ -297,14 +295,13 @@ TEST(PlatoonStrategicIHPPlugin, test_get_leader_front) // Use Getter to retrieve host Platoon Manager class platoon_strategic_ihp::PlatoonManager pm_ = plugin.getHostPM(); // check platoon size - EXPECT_EQ(pm_.platoon.size(), 1); + EXPECT_EQ(pm_.host_platoon_.size(), 1ul); - PlatoonMember member1 = pm_.platoon[0]; + PlatoonMember member1 = pm_.host_platoon_[0]; //check member info EXPECT_EQ(member1.staticId, "1"); - // update PM status - plugin.setToFollower(); + // PM should now be follower // update PM platoon_strategic_ihp::PlatoonManager pm1_ = plugin.getHostPM(); PlatoonMember platoon_leader = pm1_.getDynamicLeader(); @@ -316,51 +313,45 @@ TEST(PlatoonStrategicIHPPlugin, test_get_leader_front) // test platoon size and get leader platoon ID TEST(PlatoonManagerTestFrontJoin, test2_front) { - platoon_strategic_ihp::PlatoonMember* member = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); // dtd: 0.1m, smaller downtrack, at back. + platoon_strategic_ihp::PlatoonMember* member = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); // dtd: 0.1m, smaller downtrack, at back. std::vector cur_pl; cur_pl.push_back(*member); - platoon_strategic_ihp::PlatoonManager pm; - pm.platoon = cur_pl; + platoon_strategic_ihp::PlatoonManager pm(std::make_shared()); + pm.host_platoon_ = cur_pl; pm.isFollower = true; - pm.platoonSize = 1; - pm.leaderID = "0"; + pm.platoonLeaderID = "2"; pm.currentPlatoonID = "a"; std::string params = "CMDSPEED:11,DOWNTRACK:01,SPEED:11"; - ros::Time::init(); - - pm.updatesOrAddMemberInfo("2", 2.0, 1.0, 2.5); // dtd: 1.0m, larger down track, in front. + pm.updatesOrAddMemberInfo(cur_pl, "2", 2.0, 1.0, 0.0, 2.5); // dtd: 1.0m, larger down track, in front. - EXPECT_EQ(2, pm.platoon.size()); - EXPECT_EQ("2", pm.platoon[0].staticId); // sorted by dtd distance, larger downtrack means vehicle at front, hence ranked higher. + EXPECT_EQ(2ul, cur_pl.size()); + EXPECT_EQ("2", cur_pl[0].staticId); // sorted by dtd distance, larger downtrack means vehicle at front, hence ranked higher. } // add 2 member to platoon, test size TEST(PlatoonManagerTestFrontJoin, test3_front) { - // note: platoon list now include leading vehicle. So platoon_size = platoon.size() , here expect platoon_size == 2. - platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); - platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + // note: platoon list now include leading vehicle. So platoon_size = host_platoon_.size() , here expect platoon_size == 2. + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 0, 200); std::vector cur_pl; cur_pl.push_back(*member1); cur_pl.push_back(*member2); - platoon_strategic_ihp::PlatoonManager pm; - pm.platoon = cur_pl; + platoon_strategic_ihp::PlatoonManager pm(std::make_shared()); + pm.host_platoon_ = cur_pl; pm.isFollower = false; - pm.platoonSize = 2; - pm.leaderID = "0"; + pm.platoonLeaderID = "0"; pm.currentPlatoonID = "a"; - ros::Time::init(); - - int res = pm.getTotalPlatooningSize(); + int res = pm.getHostPlatoonSize(); EXPECT_EQ(2, res); } @@ -368,24 +359,23 @@ TEST(PlatoonManagerTestFrontJoin, test3_front) // test APF for 3 vehicles TEST(PlatoonManagerTestFrontJoin, test4_front) { - platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 100); - platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 200); + platoon_strategic_ihp::PlatoonMember* member1 = new platoon_strategic_ihp::PlatoonMember("1", 1.0, 1.1, 0.1, 0, 100); + platoon_strategic_ihp::PlatoonMember* member2 = new platoon_strategic_ihp::PlatoonMember("2", 2.0, 2.1, 0.2, 0, 200); std::vector cur_pl; cur_pl.push_back(*member1); cur_pl.push_back(*member2); - platoon_strategic_ihp::PlatoonManager pm; - pm.platoon = cur_pl; + platoon_strategic_ihp::PlatoonManager pm(std::make_shared()); + pm.host_platoon_ = cur_pl; pm.isFollower = true; - pm.platoonSize = 2; - pm.leaderID = "0"; + pm.platoonLeaderID = "0"; pm.currentPlatoonID = "a"; - ros::Time::init(); - int res = pm.allPredecessorFollowing(); EXPECT_EQ(0, res); } + +} // Namespace required for FRIEND_TEST to allow access to private members diff --git a/route_following_plugin/config/parameters.yaml b/route_following_plugin/config/parameters.yaml index 2b2c8ade12..851cabdb7a 100644 --- a/route_following_plugin/config/parameters.yaml +++ b/route_following_plugin/config/parameters.yaml @@ -1,5 +1,5 @@ # The minimum duration of a maneuver plan, in seconds -minimal_plan_duration: 15 +minimal_plan_duration: 15.0 # Multiplier for the acceleration limit which will be used for formulating the stopping maneuver # This multiplier should be a bit lower than whatever plugin will be used to bring the vehicle to a stop to provide buffer in planning diff --git a/stop_and_wait_plugin/CMakeLists.txt b/stop_and_wait_plugin/CMakeLists.txt index 835e74a1d9..6c9206761e 100644 --- a/stop_and_wait_plugin/CMakeLists.txt +++ b/stop_and_wait_plugin/CMakeLists.txt @@ -48,7 +48,7 @@ ament_auto_add_executable(${node_exec} ) # Register component -rclcpp_components_register_nodes(${worker_lib} "stop_and_wait_plugin::StopandWaitNode") +rclcpp_components_register_nodes(${node_lib} "stop_and_wait_plugin::StopandWaitNode") # All locally created targets will need to be manually linked # ament auto will handle linking of external dependencies diff --git a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp index bb89dbf4fb..78e0a5988b 100644 --- a/subsystem_controllers/src/guidance_controller/guidance_controller.cpp +++ b/subsystem_controllers/src/guidance_controller/guidance_controller.cpp @@ -116,6 +116,7 @@ namespace subsystem_controllers try { plugin_manager_->configure(); // Only checking required nodes. Other node failure tracked by activity status } catch(const std::runtime_error& e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error configuring plugin manager " << e.what()); success = false; } @@ -149,6 +150,7 @@ namespace subsystem_controllers try { plugin_manager_->activate(); // Only checking required nodes. Other node failure tracked by activity status } catch(const std::runtime_error& e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error activating plugin manager " << e.what()); success = false; } @@ -180,6 +182,7 @@ namespace subsystem_controllers try { plugin_manager_->deactivate(); // Only checking required nodes. Other node failure tracked by activity status } catch(const std::runtime_error& e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error deactivating plugin manager " << e.what()); success = false; } @@ -210,6 +213,7 @@ namespace subsystem_controllers try { plugin_manager_->cleanup(); // Only checking required nodes. Other node failure tracked by activity status } catch(const std::runtime_error& e) { + RCLCPP_ERROR_STREAM(get_logger(), "Error cleaning up plugin manager " << e.what()); success = false; } diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index edee75dd54..702bb5eeeb 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -50,14 +50,16 @@ namespace subsystem_controllers bool is_ros1 = ros2_initial_plugins_.find(p) == ros2_initial_plugins_.end(); Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true, is_ros1); em_.update_entry(e); - plugin_lifecycle_mgr_->add_managed_node(p); + if (!is_ros1) + plugin_lifecycle_mgr_->add_managed_node(p); } for (const auto& p : auto_activated_plugins_) { bool is_ros1 = ros2_initial_plugins_.find(p) == ros2_initial_plugins_.end(); Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true, is_ros1); em_.update_entry(e); - plugin_lifecycle_mgr_->add_managed_node(p); + if (!is_ros1) + plugin_lifecycle_mgr_->add_managed_node(p); } } From 04e2ddddf2aadc2041383c12b50cf0fe66eef2fb Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Mon, 22 Aug 2022 13:49:52 -0700 Subject: [PATCH 076/165] Fix applied accel multipliers --- ...ight_controlled_intersection_tactical_plugin_node.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index d4c780e9c2..8321eb5814 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -46,10 +46,6 @@ namespace light_controlled_intersection_tactical_plugin config_.lateral_accel_limit = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); config_.vehicle_accel_limit = declare_parameter("vehicle_acceleration_limit", config_.vehicle_accel_limit); config_.vehicle_decel_limit = declare_parameter("vehicle_deceleration_limit", config_.vehicle_decel_limit); - - config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; - config_.vehicle_accel_limit = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; - config_.vehicle_decel_limit = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; } rcl_interfaces::msg::SetParametersResult LightControlledIntersectionTransitPluginNode::parameter_update_callback(const std::vector ¶meters) @@ -111,6 +107,11 @@ namespace light_controlled_intersection_tactical_plugin get_parameter("vehicle_acceleration_limit", config_.vehicle_accel_limit); get_parameter("vehicle_deceleration_limit", config_.vehicle_decel_limit); + // Use the configured multipliers to update the accel limits + config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; + config_.vehicle_accel_limit = config_.vehicle_accel_limit * config_.vehicle_accel_limit_multiplier; + config_.vehicle_decel_limit = config_.vehicle_decel_limit * config_.vehicle_decel_limit_multiplier; + // Register runtime parameter update callback add_on_set_parameters_callback(std::bind(&LightControlledIntersectionTransitPluginNode::parameter_update_callback, this, std_ph::_1)); From 79f5a4b8c39095047eafa89b28344ac712c7d906 Mon Sep 17 00:00:00 2001 From: JonSmet <34752540+JonSmet@users.noreply.github.com> Date: Mon, 22 Aug 2022 16:50:47 -0400 Subject: [PATCH 077/165] Fix carma package build error (#1901) --- carma/CMakeLists.txt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/carma/CMakeLists.txt b/carma/CMakeLists.txt index ea905a710e..173302aba8 100644 --- a/carma/CMakeLists.txt +++ b/carma/CMakeLists.txt @@ -49,9 +49,6 @@ if(${ROS_VERSION} EQUAL 1) # ROS 1 scripts rviz DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - install(DIRECTORY - ui/config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ui) else() # ROS2 @@ -60,7 +57,7 @@ else() # ROS2 ament_auto_package( - INSTALL_TO_SHARE config launch routes scripts rviz ui + INSTALL_TO_SHARE config launch routes scripts rviz ) endif() From e9751ff781e712c7aa9c4fdde1c41bb5e3e1ae0c Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Mon, 22 Aug 2022 13:59:16 -0700 Subject: [PATCH 078/165] Add TODO comment for unit tests --- .../test/node_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/light_controlled_intersection_tactical_plugin/test/node_test.cpp b/light_controlled_intersection_tactical_plugin/test/node_test.cpp index 51eddba9b8..37915aace9 100644 --- a/light_controlled_intersection_tactical_plugin/test/node_test.cpp +++ b/light_controlled_intersection_tactical_plugin/test/node_test.cpp @@ -24,6 +24,8 @@ namespace light_controlled_intersection_transit_plugin { + // TODO: The package requires additional unit tests to improve unit test coverage. These unit tests will be created + // in a follow-on story. TEST(LCITacticalPluginTest, apply_accel_cruise_decel_speed_profile_test) { From f010bf9e97c81947ba211b9201b77855b5c739bb Mon Sep 17 00:00:00 2001 From: Anish_deva <51463994+adev4a@users.noreply.github.com> Date: Tue, 23 Aug 2022 06:11:01 -0700 Subject: [PATCH 079/165] Feature/ros2 localization manager (#1872) This PR includes updates to port the localization_manager node to ROS2 and to launch it from the carma_src.launch.py launch file * fix Co-authored-by: Michael McConnell Co-authored-by: Michael McConnell --- carma/launch/localization.launch | 5 - carma/launch/localization.launch.py | 27 +- docker/checkout.bash | 3 + localization_manager/CMakeLists.txt | 112 ++-- localization_manager/config/parameters.yaml | 2 +- .../LocalizationManager.h | 169 ------ .../LocalizationManager.hpp | 190 +++++++ .../LocalizationManagerConfig.h | 47 -- .../LocalizationManagerConfig.hpp | 69 +++ .../LocalizationTransitionTable.h | 89 --- .../LocalizationTransitionTable.hpp | 91 ++++ ...alizationTypes.h => LocalizationTypes.hpp} | 15 +- .../localization_manager_node.hpp | 112 ++++ .../localization_manager/localizer_node.h | 105 ---- .../launch/localization_manager.launch | 18 - .../launch/localization_manager.py | 68 +++ localization_manager/package.xml | 55 +- .../src/LocalizationManager.cpp | 367 +++++++------ .../src/LocalizationTransitionTable.cpp | 104 ++-- .../src/LocalizationTypes.cpp | 46 +- .../src/localization_manager_node.cpp | 146 +++++ localization_manager/src/localizer_node.cpp | 116 ---- localization_manager/src/main.cpp | 23 +- .../test/TestLocalizationManager.cpp | 514 +++++++++--------- .../test/TestLocalizationTransitionTable.cpp | 15 +- .../test/TestLocalizationTypes.cpp | 40 +- localization_manager/test/TestMain.cpp | 23 +- 27 files changed, 1408 insertions(+), 1163 deletions(-) delete mode 100644 localization_manager/include/localization_manager/LocalizationManager.h create mode 100644 localization_manager/include/localization_manager/LocalizationManager.hpp delete mode 100644 localization_manager/include/localization_manager/LocalizationManagerConfig.h create mode 100644 localization_manager/include/localization_manager/LocalizationManagerConfig.hpp delete mode 100644 localization_manager/include/localization_manager/LocalizationTransitionTable.h create mode 100644 localization_manager/include/localization_manager/LocalizationTransitionTable.hpp rename localization_manager/include/localization_manager/{LocalizationTypes.h => LocalizationTypes.hpp} (87%) create mode 100644 localization_manager/include/localization_manager/localization_manager_node.hpp delete mode 100644 localization_manager/include/localization_manager/localizer_node.h delete mode 100644 localization_manager/launch/localization_manager.launch create mode 100644 localization_manager/launch/localization_manager.py create mode 100644 localization_manager/src/localization_manager_node.cpp delete mode 100644 localization_manager/src/localizer_node.cpp diff --git a/carma/launch/localization.launch b/carma/launch/localization.launch index bc365759ce..c7f23d37a6 100755 --- a/carma/launch/localization.launch +++ b/carma/launch/localization.launch @@ -129,11 +129,6 @@ - - - - - diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py index 414a5527c5..b1e422f52f 100644 --- a/carma/launch/localization.launch.py +++ b/carma/launch/localization.launch.py @@ -56,6 +56,9 @@ def generate_launch_description(): gnss_to_map_convertor_param_file = os.path.join( get_package_share_directory('gnss_to_map_convertor'), 'config/parameters.yaml') + localization_manager_convertor_param_file = os.path.join( + get_package_share_directory('localization_manager'), 'config/parameters.yaml') + gnss_to_map_convertor_container = ComposableNodeContainer( package='carma_ros2_utils', name='gnss_to_map_convertor_container', @@ -79,6 +82,27 @@ def generate_launch_description(): ) ]) + localization_manager_container = ComposableNodeContainer( + package='carma_ros2_utils', + name='localization_manager_container', + executable='carma_component_container_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='localization_manager', + plugin='localization_manager::Node', + name='localization_manager', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('localization_manager', env_log_levels) } + ], + remappings=[ + + ], + parameters=[ localization_manager_convertor_param_file ] + ) + ]) + # subsystem_controller which orchestrates the lifecycle of this subsystem's components subsystem_controller = Node( package='subsystem_controllers', @@ -92,5 +116,6 @@ def generate_launch_description(): return LaunchDescription([ declare_subsystem_controller_param_file_arg, subsystem_controller, - gnss_to_map_convertor_container + gnss_to_map_convertor_container, + localization_manager_container ]) diff --git a/docker/checkout.bash b/docker/checkout.bash index 464faf0157..66eb8395c8 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -50,6 +50,9 @@ else git clone --depth=1 https://github.com/usdot-fhwa-stol/carma-messenger.git --branch develop 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 + # add astuff messages # 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 diff --git a/localization_manager/CMakeLists.txt b/localization_manager/CMakeLists.txt index 743b9a77bc..89793e9376 100644 --- a/localization_manager/CMakeLists.txt +++ b/localization_manager/CMakeLists.txt @@ -1,5 +1,4 @@ -# -# Copyright (C) 2018-2021 LEIDOS. +# 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 @@ -12,89 +11,70 @@ # 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 2.8.3) +cmake_minimum_required(VERSION 3.5) project(localization_manager) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - roscpp - std_msgs - tf2 - tf2_ros - tf2_geometry_msgs - autoware_msgs - carma_utils - cav_msgs -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) +# Name build targets +set(node_exec localization_manager_node_exec) +set(node_lib localization_manager_node) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - CATKIN_DEPENDS geometry_msgs roscpp std_msgs tf2 tf2_ros tf2_geometry_msgs autoware_msgs carma_utils cav_msgs - DEPENDS Boost +# Includes +include_directories( + include ) -########### -## Build ## -########### +# Build +ament_auto_add_library(${node_lib} SHARED + src/LocalizationTypes.cpp + src/LocalizationTransitionTable.cpp + src/LocalizationManager.cpp + src/localization_manager_node.cpp +) -include_directories( - include - ${catkin_INCLUDE_DIRS} +ament_auto_add_executable(${node_exec} + src/main.cpp ) -add_library( ${PROJECT_NAME} - src/LocalizationManager.cpp - src/LocalizationTransitionTable.cpp - src/LocalizationTypes.cpp) +# Register component +rclcpp_components_register_nodes(${node_lib} "localization_manager::Node") -target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} +) +# Testing +if(BUILD_TESTING) -add_executable( ${PROJECT_NAME}_node - src/localizer_node.cpp - src/main.cpp) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + ament_add_gtest(test_localization_manager + test/TestLocalizationManager.cpp + test/TestLocalizationTransitionTable.cpp + test/TestLocalizationTypes.cpp + test/TestMain.cpp -############# -## Install ## -############# + ) -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) + ament_target_dependencies(test_localization_manager ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -## Install C++ -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + target_link_libraries(test_localization_manager ${node_lib}) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +endif() -############# -## Testing ## -############# -catkin_add_gmock(${PROJECT_NAME}-test test/TestMain.cpp - test/TestLocalizationTransitionTable.cpp - test/TestLocalizationManager.cpp - test/TestLocalizationTypes.cpp -) -target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES}) +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) \ No newline at end of file diff --git a/localization_manager/config/parameters.yaml b/localization_manager/config/parameters.yaml index 3c7a8cc5d4..db71c0cb73 100644 --- a/localization_manager/config/parameters.yaml +++ b/localization_manager/config/parameters.yaml @@ -39,4 +39,4 @@ sequential_timesteps_until_gps_operation: 5 # 2 - AUTO select between NDT and GNSS with GNSS timeout # 3 - AUTO select between NDT and GNSS without GNSS timeout # 4 - GNSS only with NDT initialization -localization_mode: 4 +localization_mode: 4 \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationManager.h b/localization_manager/include/localization_manager/LocalizationManager.h deleted file mode 100644 index 6b072c5f32..0000000000 --- a/localization_manager/include/localization_manager/LocalizationManager.h +++ /dev/null @@ -1,169 +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 -#include -#include -#include -#include -#include -#include -#include -#include "LocalizationTypes.h" -#include "LocalizationManagerConfig.h" -#include "LocalizationTransitionTable.h" - -namespace localizer -{ -/** - * \brief Primary logic class for the localization manager node. - */ -class LocalizationManager -{ -public: - using PosePublisher = std::function; - using ManagedInitialPosePublisher = std::function; - using StatePublisher = std::function; - using TimerUniquePtr = std::unique_ptr; - - - /** - * \brief Constructor - * - * \param pose_pub A callback to trigger publication of the selected pose - * \param state_pub A callback to trigger publication of the localization state - * \param initialpose_pub A callback to trigger publication of the intial pose - * \param config The configuration settings to use for this manager - * \param timer_factory A pointer to a timer factory to support dependency injection of timing functionality for use - * in unit testing - */ - LocalizationManager(PosePublisher pose_pub, StatePublisher state_pub, - ManagedInitialPosePublisher initialpose_pub, - const LocalizationManagerConfig& config, - std::unique_ptr timer_factory); - - /** - * \brief Callback for new GNSS messages - * - * \param msg The pose of vehicle in map frame provided by GNSS - */ - void gnssPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); - - /** - * \brief Synced callback for the NDT reported pose and status messages - * - * \param pose The pose reported by NDT matching of the vehicle in the map frame - * \param stats The stats reported by NDT matching for the accuracy of the provided pose - */ - void poseAndStatsCallback(const geometry_msgs::PoseStampedConstPtr& pose, - const autoware_msgs::NDTStatConstPtr& stats); - - /** - * \brief Callback for SystemAlert data. Used to check for lidar failure - * - * \param alert The alert message to evaluate - */ - void systemAlertCallback(const cav_msgs::SystemAlertConstPtr& alert); - - /** - * \brief Callback for the initial pose provided by Rviz or some external localization initializer - * - * \param msg The initial pose message - */ - void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); - - /** - * \brief Timer callback that controls the publication of the selected pose and status report - * - */ - void posePubTick(const ros::TimerEvent& te); - - /** - * \brief Callback for when a new state was triggered. Allows creation of entry/exit behavior for states - * - * \param prev_state The previous state - * \param new_state The new current state. This should never equal prev_state. - * \param signal The signal that triggered the state transition - */ - void stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal); - - /** - * \brief Callback for timeouts. Used to trigger timeout signals for the state machine. - * - * \param event Details of the timing trigger - * \param origin_state The state that was the current state when the timer that triggered this callback was setup - */ - void timerCallback(const ros::TimerEvent& event, const LocalizationState origin_state); - - /** - * \brief Returns the current localization state - * - * \return The current localization state - */ - LocalizationState getState() const; - -private: - //! The set of strings which mark a lidar failure in a system alert message - static const std::unordered_set LIDAR_FAILURE_STRINGS; // Static const container defined in cpp file - - PosePublisher pose_pub_; - StatePublisher state_pub_; - ManagedInitialPosePublisher initialpose_pub_; - - LocalizationManagerConfig config_; - - std::unique_ptr timer_factory_; - - LocalizationTransitionTable transition_table_; - - boost::optional prev_ndt_stamp_; - - TimerUniquePtr current_timer_; - int lidar_init_sequential_timesteps_counter_ = 0; - bool is_sequential_ = false; - std::vector expired_timers_; - boost::optional last_raw_gnss_value_; - boost::optional gnss_offset_; - boost::optional current_pose_; - - /** - * \brief Helper function to both compute the NDT Frequency and update the previous pose timestamp - * - * \param new_stamp The new pose timestamp - * - * \return The computed instantaneous frequency in Hz - */ - double computeNDTFreq(const ros::Time& new_stamp); - - /** - * \brief Helper function to compute the instantaneous frequency between two times - * - * \param old_stamp The old timestamp - * \param new_stamp The new timestamp - * - * \return The computed instantaneous frequency in Hz - */ - double computeFreq(const ros::Time& old_stamp, const ros::Time& new_stamp) const; -}; - -} // namespace localizer diff --git a/localization_manager/include/localization_manager/LocalizationManager.hpp b/localization_manager/include/localization_manager/LocalizationManager.hpp new file mode 100644 index 0000000000..ecf057d19c --- /dev/null +++ b/localization_manager/include/localization_manager/LocalizationManager.hpp @@ -0,0 +1,190 @@ +/* + * 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. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "localization_manager/LocalizationTypes.hpp" +#include "localization_manager/LocalizationManagerConfig.hpp" +#include "localization_manager/LocalizationTransitionTable.hpp" + +namespace localization_manager +{ +/** + * \brief Primary logic class for the localization manager node. + */ +class LocalizationManager +{ + +public: + using PosePublisher = std::function; + using ManagedInitialPosePublisher = std::function; + using StatePublisher = std::function; + using TimerUniquePtr = std::unique_ptr; + + /** + * \brief Constructor + * + * \param pose_pub A callback to trigger publication of the selected pose + * \param state_pub A callback to trigger publication of the localization state + * \param initialpose_pub A callback to trigger publication of the intial pose + * \param config The configuration settings to use for this manager + * \param logger Logger interface of node calling manager object + * \param node_timers Timer interface of node calling manager object + */ + LocalizationManager(PosePublisher pose_pub, StatePublisher state_pub, + ManagedInitialPosePublisher initialpose_pub, + const LocalizationManagerConfig& config, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, + std::unique_ptr timer_factory); + + /** + * \brief Callback for new GNSS messages + * + * \param msg The pose of vehicle in map frame provided by GNSS + */ + void gnssPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + /** + * \brief Synced callback for the NDT reported pose and status messages + * + * \param pose The pose reported by NDT matching of the vehicle in the map frame + * \param stats The stats reported by NDT matching for the accuracy of the provided pose + */ + void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, + const autoware_msgs::msg::NDTStat::ConstPtr stats); + + /** + * \brief Callback for SystemAlert data. Used to check for lidar failure + * + * \param alert The alert message to evaluate + */ + void systemAlertCallback(const carma_msgs::msg::SystemAlert::SharedPtr alert); + + /** + * \brief Callback for the initial pose provided by Rviz or some external localization initializer + * + * \param msg The initial pose message + */ + void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * \brief Timer callback that controls the publication of the selected pose and status report + * + */ + void posePubTick(); + + /** + * \brief Callback for when a new state was triggered. Allows creation of entry/exit behavior for states + * + * \param prev_state The previous state + * \param new_state The new current state. This should never equal prev_state. + * \param signal The signal that triggered the state transition + */ + void stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal); + + /** + * \brief Callback for timeouts. Used to trigger timeout signals for the state machine. + * + * \param origin_state The state that was the current state when the timer that triggered this callback was setup + */ + void timerCallback(const LocalizationState origin_state); + + /** + * \brief Returns the current localization state + * + * \return The current localization state + */ + LocalizationState getState() const; + + /** + * @brief Clears the expired timers from the memory of this scheduler + */ + void clearTimers(); + +private: + //! The set of strings which mark a lidar failure in a system alert message + static const std::unordered_set LIDAR_FAILURE_STRINGS; // Static const container defined in cpp file + + PosePublisher pose_pub_; + StatePublisher state_pub_; + ManagedInitialPosePublisher initialpose_pub_; + + LocalizationManagerConfig config_; + + LocalizationTransitionTable transition_table_; + + boost::optional prev_ndt_stamp_; + + int lidar_init_sequential_timesteps_counter_ = 0; + bool is_sequential_ = false; + boost::optional last_raw_gnss_value_; + boost::optional gnss_offset_; + boost::optional current_pose_; + + // Logger interface + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; + + // Using timer factory + std::mutex mutex_; + std::unique_ptr timer_factory_; + std::unordered_map> timers_; + TimerUniquePtr current_timer_; + uint32_t next_id_ = 0; // Timer id counter + uint32_t current_timer_id_; + rcl_clock_type_t timer_clock_type_ = RCL_SYSTEM_TIME; + + + + /** + * \brief Helper function to both compute the NDT Frequency and update the previous pose timestamp + * + * \param new_stamp The new pose timestamp + * + * \return The computed instantaneous frequency in Hz + */ + double computeNDTFreq(const rclcpp::Time& new_stamp); + + /** + * \brief Helper function to compute the instantaneous frequency between two times + * + * \param old_stamp The old timestamp + * \param new_stamp The new timestamp + * + * \return The computed instantaneous frequency in Hz + */ + double computeFreq(const rclcpp::Time& old_stamp, const rclcpp::Time& new_stamp) const; + + /** + * @brief Generates the next id to be used for a timer + * + * @return The next available timer id + */ + uint32_t nextId(); + +}; +} \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationManagerConfig.h b/localization_manager/include/localization_manager/LocalizationManagerConfig.h deleted file mode 100644 index e134024fd3..0000000000 --- a/localization_manager/include/localization_manager/LocalizationManagerConfig.h +++ /dev/null @@ -1,47 +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 "LocalizationTypes.h" - -namespace localizer -{ -//! @brief Struct to store the configuration settings for the LocalizationManager class -struct LocalizationManagerConfig -{ - //! NDT Fitness score above which the localization is considered in a degraded state - double fitness_score_degraded_threshold = 20.0; - //! NDT Fitness score above which the localization is considered in a fault state and NDT matching can no longer be - //! used. - double fitness_score_fault_threshold = 10000.0; - //! NDT solution frequency below which the localization is considered in a degraded state - double ndt_frequency_degraded_threshold = 8.0; - //! NDT solution frequency below which the localization is considered in a fault state and NDT matching can no longer - //! be used. - double ndt_frequency_fault_threshold = 0.66; - //! Timeout in ms for auto initialization. - //! If initialization cannot be completed in this time user action will be requested. - int auto_initialization_timeout = 30000; - //! Timeout in ms for GNSS only operation. Ignored when in GNSS mode. - int gnss_only_operation_timeout = 20000; - //! Integer: Maximum allowed number of sequential timesteps to let lidar initialize before switching to GPS only mode - //! NOTE: Only used in GNSS only with NDT initialization mode - int sequential_timesteps_until_gps_operation = 5; - //! GNSS Data timeout. If exceeded the system will assume the GNSS is no longer functional. Units are ms - int gnss_data_timeout = 500; - //! Localization mode to use - LocalizerMode localization_mode = LocalizerMode::AUTO_WITHOUT_TIMEOUT; -}; -} // namespace localizer \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp b/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp new file mode 100644 index 0000000000..7617437fa2 --- /dev/null +++ b/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp @@ -0,0 +1,69 @@ +#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. + */ + +#include "localization_manager/LocalizationTypes.hpp" +#include "localization_manager/LocalizationTypes.hpp" + +namespace localization_manager +{ +//! @brief Struct to store the configuration settings for the LocalizationManager class +struct LocalizationManagerConfig +{ + //! NDT Fitness score above which the localization is considered in a degraded state + double fitness_score_degraded_threshold = 20.0; + //! NDT Fitness score above which the localization is considered in a fault state and NDT matching can no longer be + //! used. + double fitness_score_fault_threshold = 10000.0; + //! NDT solution frequency below which the localization is considered in a degraded state + double ndt_frequency_degraded_threshold = 8.0; + //! NDT solution frequency below which the localization is considered in a fault state and NDT matching can no longer + //! be used. + double ndt_frequency_fault_threshold = 0.66; + //! Timeout in ms for auto initialization. + //! If initialization cannot be completed in this time user action will be requested. + int auto_initialization_timeout = 30000; + //! Timeout in ms for GNSS only operation. Ignored when in GNSS mode. + int gnss_only_operation_timeout = 20000; + //! Integer: Maximum allowed number of sequential timesteps to let lidar initialize before switching to GPS only mode + //! NOTE: Only used in GNSS only with NDT initialization mode + int sequential_timesteps_until_gps_operation = 5; + //! GNSS Data timeout. If exceeded the system will assume the GNSS is no longer functional. Units are ms + int gnss_data_timeout = 500; + //! Localization mode to use + int localization_mode = static_cast(LocalizerMode::AUTO_WITHOUT_TIMEOUT); + //! Selected pose publication rate + double pose_pub_rate = 10.0; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const LocalizationManagerConfig &c) + { + output << "localization_manager::Config { " << std::endl + << "fitness_score_degraded_threshold: " << c.fitness_score_degraded_threshold << std::endl + << "fitness_score_fault_threshold: " << c.fitness_score_fault_threshold << std::endl + << "ndt_frequency_degraded_threshold: " << c.ndt_frequency_degraded_threshold << std::endl + << "ndt_frequency_fault_threshold: " << c.ndt_frequency_fault_threshold << std::endl + << "auto_initialization_timeout: " << c.auto_initialization_timeout << std::endl + << "gnss_only_operation_timeout: " << c.gnss_only_operation_timeout << std::endl + << "sequential_timesteps_until_gps_operation: " << c.sequential_timesteps_until_gps_operation << std::endl + << "gnss_data_timeout: " << c.gnss_data_timeout << std::endl + << "localization_mode: " << static_cast(c.localization_mode) << std::endl + << "pose_pub_rate: " << c.pose_pub_rate << std::endl + << "}" << std::endl; + return output; + } +}; +} //namespace localization_manager \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationTransitionTable.h b/localization_manager/include/localization_manager/LocalizationTransitionTable.h deleted file mode 100644 index ca70c988ef..0000000000 --- a/localization_manager/include/localization_manager/LocalizationTransitionTable.h +++ /dev/null @@ -1,89 +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 "LocalizationTypes.h" - -namespace localizer -{ -/** - * \brief Class defining the state transition table behavior for the LocalizationManager - */ -class LocalizationTransitionTable -{ -public: - using TransitionCallback = - std::function; - - /** - * \brief Constructor - * - * \param mode Defines the operational mode of the state machine which modifies some of the state transitions - */ - LocalizationTransitionTable(LocalizerMode mode); - - /** - * \brief Returns the current state - * \return Current state - */ - LocalizationState getState() const; - - /** - * \brief Trigger signal for the transition table. - * - * \param signal The signal for the transition table to evaluate - */ - void signal(LocalizationSignal signal); - - /** - * \brief Callback setting function. The provided callback will be triggered any time the current state changes to a - * new state. - * - * \param cb The callback function which will be provided with the previous state, new current state, and the signal - * which caused the transition. - */ - void setTransitionCallback(TransitionCallback cb); - -private: - //! Current state. This state should only ever be set using the setAndLogState() function. - LocalizationState state_ = LocalizationState::UNINITIALIZED; - - LocalizerMode mode_ = LocalizerMode::AUTO_WITHOUT_TIMEOUT; - - TransitionCallback transition_callback_; - - // Helper functions for processing each the provided signal based on the current state - void signalWhenUNINITIALIZED(LocalizationSignal signal); - void signalWhenINITIALIZING(LocalizationSignal signal); - void signalWhenOPERATIONAL(LocalizationSignal signal); - void signalWhenDEGRADED(LocalizationSignal signal); - void signalWhenDEGRADED_NO_LIDAR_FIX(LocalizationSignal signal); - void signalWhenAWAIT_MANUAL_INITIALIZATION(LocalizationSignal signal); - - /** - * \brief Helper function for logging the provide signal - * \param signal The signal to be logged - */ - void logDebugSignal(LocalizationSignal signal) const; - - /** - * \brief Function to change the current state and log the details of the transition. - * - * \param new_state The state to set. - * \param source_signal The signal which caused the new_state to be set - */ - void setAndLogState(LocalizationState new_state, LocalizationSignal source_signal); -}; -} // namespace localizer \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationTransitionTable.hpp b/localization_manager/include/localization_manager/LocalizationTransitionTable.hpp new file mode 100644 index 0000000000..077058874b --- /dev/null +++ b/localization_manager/include/localization_manager/LocalizationTransitionTable.hpp @@ -0,0 +1,91 @@ +/* + * 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. + */ + +#pragma once + +#include "LocalizationTypes.hpp" + +namespace localization_manager +{ +/** + * \brief Class defining the state transition table behavior for the LocalizationManager + */ +class LocalizationTransitionTable +{ +public: + using TransitionCallback = + std::function; + + /** + * \brief Constructor + * + * \param mode Defines the operational mode of the state machine which modifies some of the state transitions + */ + LocalizationTransitionTable(LocalizerMode mode); + + /** + * \brief Returns the current state + * \return Current state + */ + LocalizationState getState() const; + + /** + * \brief Trigger signal for the transition table. + * + * \param signal The signal for the transition table to evaluate + */ + void signal(LocalizationSignal signal); + + /** + * \brief Callback setting function. The provided callback will be triggered any time the current state changes to a + * new state. + * + * \param cb The callback function which will be provided with the previous state, new current state, and the signal + * which caused the transition. + */ + void setTransitionCallback(TransitionCallback cb); + +private: + //! Current state. This state should only ever be set using the setAndLogState() function. + LocalizationState state_ = LocalizationState::UNINITIALIZED; + + LocalizerMode mode_ = LocalizerMode::AUTO_WITHOUT_TIMEOUT; + + TransitionCallback transition_callback_; + + // Helper functions for processing each the provided signal based on the current state + void signalWhenUNINITIALIZED(LocalizationSignal signal); + void signalWhenINITIALIZING(LocalizationSignal signal); + void signalWhenOPERATIONAL(LocalizationSignal signal); + void signalWhenDEGRADED(LocalizationSignal signal); + void signalWhenDEGRADED_NO_LIDAR_FIX(LocalizationSignal signal); + void signalWhenAWAIT_MANUAL_INITIALIZATION(LocalizationSignal signal); + + /** + * \brief Helper function for logging the provide signal + * \param signal The signal to be logged + */ + void logDebugSignal(LocalizationSignal signal) const; + + /** + * \brief Function to change the current state and log the details of the transition. + * + * \param new_state The state to set. + * \param source_signal The signal which caused the new_state to be set + */ + void setAndLogState(LocalizationState new_state, LocalizationSignal source_signal); +}; +} //namespace localization_manager \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationTypes.h b/localization_manager/include/localization_manager/LocalizationTypes.hpp similarity index 87% rename from localization_manager/include/localization_manager/LocalizationTypes.h rename to localization_manager/include/localization_manager/LocalizationTypes.hpp index ff48971b0f..332ee8c211 100644 --- a/localization_manager/include/localization_manager/LocalizationTypes.h +++ b/localization_manager/include/localization_manager/LocalizationTypes.hpp @@ -1,6 +1,6 @@ #pragma once /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -16,9 +16,10 @@ */ #include -#include +#include +#include -namespace localizer +namespace localization_manager { //! @brief Enum describing the possible operational modes of the LocalizationManager enum LocalizerMode @@ -29,6 +30,7 @@ enum LocalizerMode AUTO_WITHOUT_TIMEOUT = 3, // NDT operation with support for GPS fallback that will not timeout GNSS_WITH_NDT_INIT = 4, // GNSS only operation with NDT initialization, switching to GNSS after 5 sequential timesteps of OPERATIONAL NDT }; + /** * \brief Stream operator for LocalizerMode enum. */ @@ -44,10 +46,12 @@ enum class LocalizationState DEGRADED_NO_LIDAR_FIX, AWAIT_MANUAL_INITIALIZATION, }; + /** * \brief Stream operator for LocalizationState enum. */ std::ostream& operator<<(std::ostream& os, LocalizationState s); + /** * \brief Helper function to convert LocalizationState objects into LocalizationStatusReport ROS messages * @@ -56,7 +60,7 @@ std::ostream& operator<<(std::ostream& os, LocalizationState s); * * \return The initialized report message */ -cav_msgs::LocalizationStatusReport stateToMsg(LocalizationState state, const ros::Time& stamp); +carma_localization_msgs::msg::LocalizationStatusReport stateToMsg(LocalizationState state, const rclcpp::Time& stamp); //! @brief Enum describing the possible signals to change the current LocalizationState @@ -71,9 +75,10 @@ enum class LocalizationSignal LIDAR_INITIALIZED_SWITCH_TO_GPS, GNSS_DATA_TIMEOUT }; + /** * \brief Stream operator for LocalizationSignal enum. */ std::ostream& operator<<(std::ostream& os, LocalizationSignal s); -} // namespace localizer \ No newline at end of file +} //namespace localization_manager \ No newline at end of file diff --git a/localization_manager/include/localization_manager/localization_manager_node.hpp b/localization_manager/include/localization_manager/localization_manager_node.hpp new file mode 100644 index 0000000000..41af9d48ff --- /dev/null +++ b/localization_manager/include/localization_manager/localization_manager_node.hpp @@ -0,0 +1,112 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include "localization_manager/LocalizationManagerConfig.hpp" +#include "localization_manager/LocalizationManager.hpp" +#include "localization_manager/LocalizationTypes.hpp" +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace localization_manager +{ + /** + * \brief Core execution node for this package + */ + + class Node : public carma_ros2_utils::CarmaLifecycleNode + { + private: + // Subscribers + message_filters::Subscriber ndt_pose_sub_; + message_filters::Subscriber ndt_score_sub_; + carma_ros2_utils::SubPtr gnss_pose_sub_; + carma_ros2_utils::SubPtr initialpose_sub_; + carma_ros2_utils::SubPtr system_alert_sub_; + + // Publishers + carma_ros2_utils::PubPtr pose_pub_; + carma_ros2_utils::PubPtr state_pub_; + carma_ros2_utils::PubPtr managed_initial_pose_pub_; + + // Timers + rclcpp::TimerBase::SharedPtr pose_timer_; + + // Node configuration + LocalizationManagerConfig config_; + + // Worker object + std::unique_ptr manager_; + + // Message filters policies (TimeSynchronizer by default uses ExactTime Policy) + typedef message_filters::TimeSynchronizer TimeSynchronizer; + std::shared_ptr pose_stats_synchronizer_; + + public: + /** + * \brief Node constructor + */ + explicit Node(const rclcpp::NodeOptions &); + + /** + * \brief Callback to publish the selected pose + * \param msg The pose to publish + */ + void publishPoseStamped(const geometry_msgs::msg::PoseStamped& msg) const; + + /** + * \brief Callback to publish the provided localization status report + * \param msg The report to publish + */ + void publishStatus(const carma_localization_msgs::msg::LocalizationStatusReport& msg) const; + + /** + * \brief Callback to publish the initial pose deemed suitable to intialize NDT + * \param msg The msg to publish + */ + void publishManagedInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) const; + + /** + * \brief Synchronized callback for pose and stats data for usage with message_filters. + * Provides exception handling. + * \param pose The received pose message + * \param stats The received stats message + */ + void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, + const autoware_msgs::msg::NDTStat::ConstPtr stats); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + + }; +} \ No newline at end of file diff --git a/localization_manager/include/localization_manager/localizer_node.h b/localization_manager/include/localization_manager/localizer_node.h deleted file mode 100644 index 95d34b9101..0000000000 --- a/localization_manager/include/localization_manager/localizer_node.h +++ /dev/null @@ -1,105 +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 -#include -#include -#include -#include -#include -#include "LocalizationTypes.h" -#include "LocalizationManagerConfig.h" -#include "LocalizationManager.h" - -namespace localizer -{ -/** - * \brief Node class for this package - */ -class Localizer -{ -public: - /** - * \brief Default constructor - */ - Localizer()=default; - - /** - * \brief Primary entry function for node execution. Will not exit until node shutdown. - */ - void run(); - - /** - * \brief Callback to publish the selected pose - * \param msg The pose to publish - */ - void publishPoseStamped(const geometry_msgs::PoseStamped& msg) const; - - /** - * \brief Callback to publish the provided localization status report - * \param msg The report to publish - */ - void publishStatus(const cav_msgs::LocalizationStatusReport& msg) const; - - /** - * \brief Callback to publish the initial pose deemed suitable to intialize NDT - * \param msg The msg to publish - */ - void publishManagedInitialPose(const geometry_msgs::PoseWithCovarianceStamped& msg) const; - - - /** - * \brief Synchronized callback for pose and stats data for usage with message_filters. - * Provides exception handling. - * \param pose The received pose message - * \param stats The received stats message - */ - void poseAndStatsCallback(const geometry_msgs::PoseStampedConstPtr& pose, - const autoware_msgs::NDTStatConstPtr& stats) const; - -private: - // node handles - ros::CARMANodeHandle nh_, pnh_; - - // subscribers - ros::Subscriber ndt_pose_sub_; - ros::Subscriber ndt_score_sub_; - ros::Subscriber gnss_pose_sub_; - ros::Subscriber initialpose_sub_; - - // publisher - ros::Publisher pose_pub_; - ros::Publisher state_pub_; - ros::Publisher managed_initial_pose_pub_; - ros::Timer pose_timer_; - double pose_pub_rate_ = 10.0; - - std::unique_ptr manager_; // Worker object - - // Message filters policies - typedef message_filters::sync_policies::ExactTime - PoseStatsSyncPolicy; - typedef message_filters::Synchronizer PoseStatsSynchronizer; -}; - -} // namespace localizer diff --git a/localization_manager/launch/localization_manager.launch b/localization_manager/launch/localization_manager.launch deleted file mode 100644 index 45447f3b4e..0000000000 --- a/localization_manager/launch/localization_manager.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - diff --git a/localization_manager/launch/localization_manager.py b/localization_manager/launch/localization_manager.py new file mode 100644 index 0000000000..64b1cdd866 --- /dev/null +++ b/localization_manager/launch/localization_manager.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA localization_manager node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('localization_manager'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='localization_manager_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='localization_manager', + plugin='localization_manager::Node', + name='localization_manager_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/localization_manager/package.xml b/localization_manager/package.xml index bd60873587..3045b8f8fe 100644 --- a/localization_manager/package.xml +++ b/localization_manager/package.xml @@ -1,14 +1,11 @@ - localization_manager - 3.3.0 - The localizer package provides ndt or GNSS localization selection and publishes pose info as tf2 transformation messages - carma - Apache License 2.0 - carma - catkin - geometry_msgs - roscpp - rostest - std_msgs - tf2 - tf2_ros - tf2_geometry_msgs - autoware_msgs - carma_utils - cav_msgs - carma_cmake_common - + localization_manager + 4.0.0 + The localization_manager package + + carma + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + carma_msgs + geometry_msgs + carma_localization_msgs + autoware_msgs + message_filters_humble + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + + \ No newline at end of file diff --git a/localization_manager/src/LocalizationManager.cpp b/localization_manager/src/LocalizationManager.cpp index 9dd1005374..4151908e8e 100644 --- a/localization_manager/src/LocalizationManager.cpp +++ b/localization_manager/src/LocalizationManager.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -16,128 +16,134 @@ #include #include -#include "localization_manager/LocalizationManager.h" +#include "localization_manager/LocalizationManager.hpp" -namespace localizer +namespace localization_manager { // Initialize static values -const std::unordered_set LocalizationManager::LIDAR_FAILURE_STRINGS({ "One LIDAR Failed", "Both LIDARS Failed" }); - -LocalizationManager::LocalizationManager(PosePublisher pose_pub, - StatePublisher state_pub, ManagedInitialPosePublisher initialpose_pub, - const LocalizationManagerConfig& config, - std::unique_ptr timer_factory) - : pose_pub_(pose_pub) - , state_pub_(state_pub) - , initialpose_pub_(initialpose_pub) - , config_(config) - , timer_factory_(std::move(timer_factory)) - , transition_table_(config_.localization_mode) +const std::unordered_set LocalizationManager::LIDAR_FAILURE_STRINGS({"One LIDAR Failed", "Both LIDARS Failed"}); + +LocalizationManager::LocalizationManager(PosePublisher pose_pub, + StatePublisher state_pub, ManagedInitialPosePublisher initialpose_pub, + const LocalizationManagerConfig& config, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, + std::unique_ptr timer_factory) +: pose_pub_(pose_pub) +, state_pub_(state_pub) +, initialpose_pub_(initialpose_pub) +, config_(config) +, logger_(logger) +, timer_factory_(std::move(timer_factory)) +, transition_table_(static_cast(config_.localization_mode)) + { - transition_table_.setTransitionCallback(std::bind(&LocalizationManager::stateTransitionCallback, this, + + transition_table_.setTransitionCallback(std::bind(&LocalizationManager::stateTransitionCallback, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); -} + std::placeholders::_3)); + timer_clock_type_ = timer_factory_->now().get_clock_type(); + +} -double LocalizationManager::computeFreq(const ros::Time& old_stamp, const ros::Time& new_stamp) const +double LocalizationManager::computeFreq(const rclcpp::Time& old_stamp, const rclcpp::Time& new_stamp) const { - return 1.0 / (new_stamp - old_stamp).toSec(); // Convert delta to frequency (Hz = 1/s) + return 1.0 / (new_stamp - old_stamp).seconds(); // Convert delta to frequency (Hz = 1/s) } -double LocalizationManager::computeNDTFreq(const ros::Time& new_stamp) +double LocalizationManager::computeNDTFreq(const rclcpp::Time& new_stamp) { - if (!prev_ndt_stamp_) - { // Check if this is the first data point - prev_ndt_stamp_ = new_stamp; - // When no historic data is available force the frequency into the operational range - return config_.ndt_frequency_degraded_threshold * 2; - } - - if (new_stamp <= prev_ndt_stamp_) - { - ROS_ERROR_STREAM("LocalizationManager received NDT data out of order. Prev stamp was " - << prev_ndt_stamp_.get() << " new stamp is " << new_stamp); - // When invalid data is received from NDT force the frequency into the fault range - return config_.ndt_frequency_fault_threshold / 2; - } + if (!prev_ndt_stamp_) + { // Check if this is the first data point + prev_ndt_stamp_ = new_stamp; + // When no historic data is available force the frequency into the operational range + return config_.ndt_frequency_degraded_threshold * 2; + } - return computeFreq(prev_ndt_stamp_.get(), new_stamp); // Convert delta to frequency (Hz = 1/s) + if (new_stamp <= rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_)) + { + RCLCPP_ERROR_STREAM(logger_->get_logger(), "LocalizationManager received NDT data out of order. Prev stamp was " + << prev_ndt_stamp_.get().seconds() << " new stamp is " << new_stamp.seconds()); + // When invalid data is received from NDT force the frequency into the fault range + return config_.ndt_frequency_fault_threshold / 2; + } + return computeFreq(rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_), new_stamp); // Convert delta to frequency (Hz = 1/s) } -void LocalizationManager::poseAndStatsCallback(const geometry_msgs::PoseStampedConstPtr& pose, - const autoware_msgs::NDTStatConstPtr& stats) +void LocalizationManager::poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, + const autoware_msgs::msg::NDTStat::ConstPtr stats) { - double ndt_freq = computeNDTFreq(pose->header.stamp); - ROS_DEBUG_STREAM("Received pose resulting in frequency value of " << ndt_freq << " with score of " << stats->score); + double ndt_freq = computeNDTFreq(rclcpp::Time(pose->header.stamp, timer_clock_type_)); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Received pose resulting in frequency value of " << ndt_freq << " with score of " << stats->score); - if (stats->score >= config_.fitness_score_fault_threshold || ndt_freq <= config_.ndt_frequency_fault_threshold) - { - transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); - } - else if (stats->score >= config_.fitness_score_degraded_threshold || - ndt_freq <= config_.ndt_frequency_degraded_threshold) - { - transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); - } - else - { - // In GNSS_WITH_NDT_INIT mode, we shouldn't switch back to NDT and only rely on GPS - if (!(config_.localization_mode == LocalizerMode::GNSS_WITH_NDT_INIT && - transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX && - lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) - transition_table_.signal(LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE); - } + if (stats->score >= config_.fitness_score_fault_threshold || ndt_freq <= config_.ndt_frequency_fault_threshold) + { + transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); + } + else if (stats->score >= config_.fitness_score_degraded_threshold ||ndt_freq <= config_.ndt_frequency_degraded_threshold) + { + transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); + } + else + { + // In GNSS_WITH_NDT_INIT mode, we shouldn't switch back to NDT and only rely on GPS + if (!(static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && + transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX && + lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) + { + transition_table_.signal(LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE); + } + } - const LocalizationState state = transition_table_.getState(); + const LocalizationState state = transition_table_.getState(); - if (config_.localization_mode == LocalizerMode::GNSS_WITH_NDT_INIT && state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) - { - ROS_DEBUG_STREAM("lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); - if (lidar_init_sequential_timesteps_counter_ < config_.sequential_timesteps_until_gps_operation) + if (static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) { - if (is_sequential_) - lidar_init_sequential_timesteps_counter_ ++; - - is_sequential_ = true; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); + if (lidar_init_sequential_timesteps_counter_ < config_.sequential_timesteps_until_gps_operation) + { + if (is_sequential_) + lidar_init_sequential_timesteps_counter_ ++; + + is_sequential_ = true; + } + else + { + transition_table_.signal(LocalizationSignal::LIDAR_INITIALIZED_SWITCH_TO_GPS); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Switched to LIDAR_INITIALIZED_SWITCH_TO_GPS at lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ + << ", with new state: " << transition_table_.getState()); + } } - else + else if (!(static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && + state == LocalizationState::DEGRADED_NO_LIDAR_FIX && + last_raw_gnss_value_ && + lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) // don't reset counter if Lidar initialized and switched to GPS + // as it will reset state to OPERATIONAL { - transition_table_.signal(LocalizationSignal::LIDAR_INITIALIZED_SWITCH_TO_GPS); - ROS_DEBUG_STREAM("Switched to LIDAR_INITIALIZED_SWITCH_TO_GPS at lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ - << ", with new state: " << transition_table_.getState()); + is_sequential_ = false; + lidar_init_sequential_timesteps_counter_ = 0; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); } - } - else if (!(config_.localization_mode == LocalizerMode::GNSS_WITH_NDT_INIT && - state == LocalizationState::DEGRADED_NO_LIDAR_FIX && - last_raw_gnss_value_ && - lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) // don't reset counter if Lidar initialized and switched to GPS - // as it will reset state to OPERATIONAL - { - is_sequential_ = false; - lidar_init_sequential_timesteps_counter_ = 0; - ROS_DEBUG_STREAM("Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); - } - - if (state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) { - tf2::Vector3 ndt_translation(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z); - tf2::Vector3 gnss_translation(last_raw_gnss_value_->pose.position.x, last_raw_gnss_value_->pose.position.y, last_raw_gnss_value_->pose.position.z); + if (state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) { + tf2::Vector3 ndt_translation(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z); - gnss_offset_ = ndt_translation - gnss_translation; - } + tf2::Vector3 gnss_translation(last_raw_gnss_value_->pose.position.x, last_raw_gnss_value_->pose.position.y, last_raw_gnss_value_->pose.position.z); - if (state != LocalizationState::DEGRADED_NO_LIDAR_FIX) - { - - ROS_DEBUG_STREAM("Publishing mixed pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", and state" << state); - current_pose_ = *pose; - } + gnss_offset_ = ndt_translation - gnss_translation; + } - prev_ndt_stamp_ = pose->header.stamp; -} + if (state != LocalizationState::DEGRADED_NO_LIDAR_FIX) + { + + RCLCPP_DEBUG_STREAM(logger_->get_logger(),"Publishing mixed pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", and state" << state); + current_pose_ = *pose; + } -void LocalizationManager::gnssPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg) + prev_ndt_stamp_ = pose->header.stamp; +} + +void LocalizationManager::gnssPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { last_raw_gnss_value_ = *msg; // Just like ndt_matching the gnss pose is treated as an initialize signal if the system is not yet intialized @@ -145,7 +151,7 @@ void LocalizationManager::gnssPoseCallback(const geometry_msgs::PoseStampedConst lidar_init_sequential_timesteps_counter_ = 0; transition_table_.signal(LocalizationSignal::INITIAL_POSE); - geometry_msgs::PoseWithCovarianceStamped new_initial_pose; + geometry_msgs::msg::PoseWithCovarianceStamped new_initial_pose; new_initial_pose.header = msg->header; new_initial_pose.pose.pose = msg->pose; @@ -154,18 +160,18 @@ void LocalizationManager::gnssPoseCallback(const geometry_msgs::PoseStampedConst if (transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX) { - geometry_msgs::PoseStamped corrected_pose = *msg; + geometry_msgs::msg::PoseStamped corrected_pose = *msg; if (gnss_offset_) { corrected_pose.pose.position.x = corrected_pose.pose.position.x + gnss_offset_->x(); corrected_pose.pose.position.y = corrected_pose.pose.position.y + gnss_offset_->y(); corrected_pose.pose.position.z = corrected_pose.pose.position.z + gnss_offset_->z(); } - ROS_DEBUG_STREAM("Publishing GNSS pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Publishing GNSS pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); current_pose_ = corrected_pose; } } -void LocalizationManager::initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) +void LocalizationManager::initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { lidar_init_sequential_timesteps_counter_ = 0; transition_table_.signal(LocalizationSignal::INITIAL_POSE); @@ -173,7 +179,7 @@ void LocalizationManager::initialPoseCallback(const geometry_msgs::PoseWithCovar initialpose_pub_(*msg); // Forward the initial pose to the rest of the system } -void LocalizationManager::systemAlertCallback(const cav_msgs::SystemAlertConstPtr& alert) +void LocalizationManager::systemAlertCallback(const carma_msgs::msg::SystemAlert::SharedPtr alert) { if (LIDAR_FAILURE_STRINGS.find(alert->description) != LIDAR_FAILURE_STRINGS.end()) { @@ -181,92 +187,123 @@ void LocalizationManager::systemAlertCallback(const cav_msgs::SystemAlertConstPt } } -void LocalizationManager::timerCallback(const ros::TimerEvent& event, const LocalizationState origin_state) +void LocalizationManager::timerCallback(const LocalizationState origin_state) { - // If there is already a timer callback in progress or the expected state has changed then return + // If there is already a timer callback in progress or the expected state has changed then return if (origin_state != transition_table_.getState()) return; transition_table_.signal(LocalizationSignal::TIMEOUT); } -void LocalizationManager::stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, - LocalizationSignal signal) +uint32_t LocalizationManager::nextId() { - // Mark the existing timer as expired if any - if (current_timer_) - { - // NOTE unit testing of current algorithm depends on all timers being one-shot timers. - expired_timers_.push_back(std::move(current_timer_)); - } - - switch (new_state) - { - case LocalizationState::INITIALIZING: - gnss_offset_ = boost::none; - prev_ndt_stamp_ = boost::none; - - current_timer_ = timer_factory_->buildTimer( - 1, ros::Duration((double)config_.auto_initialization_timeout / 1000.0), - std::bind(&LocalizationManager::timerCallback, this, std::placeholders::_1, new_state), true); + next_id_++; + return next_id_; +} - break; - case LocalizationState::DEGRADED_NO_LIDAR_FIX: - current_timer_ = timer_factory_->buildTimer( - 1, ros::Duration((double)config_.gnss_only_operation_timeout / 1000.0), - std::bind(&LocalizationManager::timerCallback, this, std::placeholders::_1, new_state), true); +void LocalizationManager::clearTimers() +{ + std::lock_guard guard(mutex_); - break; - default: - break; - } + // Clear expired timers + auto it = timers_.begin(); + while(it != timers_.end()) + { + // Check if timer is marked for deletion + if(it->second.second) + { + // erase() function returns the iterator of the next to last deleted element + it = timers_.erase(it); + } + else{ + it++; + } + } + } -void LocalizationManager::posePubTick(const ros::TimerEvent& te) +void LocalizationManager::stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, + LocalizationSignal signal) { - // Clear any expired timers - expired_timers_.clear(); - - // Evaluate NDT Frequency if we have started receiving messages - // This check provides protection against excessively long NDT computation times that do not trigger the callback - if (prev_ndt_stamp_) - { - double freq = computeFreq(prev_ndt_stamp_.get(), ros::Time::now()); - if (freq <= config_.ndt_frequency_fault_threshold) + // Mark the expired timers as expired if any + if(current_timer_) { - transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); + // NOTE unit testing of current algorithm depends on all timers being one-shot timers. + timers_[current_timer_id_].second = true; } - else if (freq <= config_.ndt_frequency_degraded_threshold) + + + switch(new_state) { - transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); + case LocalizationState::INITIALIZING: + gnss_offset_ = boost::none; + prev_ndt_stamp_ = boost::none; + + current_timer_id_ = nextId(); + current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.auto_initialization_timeout * 1e6), + std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); + + + timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id + break; + case LocalizationState::DEGRADED_NO_LIDAR_FIX: + + current_timer_ = timer_factory_->buildTimer(nextId(), rclcpp::Duration(config_.gnss_only_operation_timeout * 1e6), + std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); + + timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id + + break; + default: + break; } - } +} - // check if last gnss time stamp is older than threshold and send the corresponding signal - if (last_raw_gnss_value_ && ros::Time::now() - last_raw_gnss_value_->header.stamp > ros::Duration(config_.gnss_data_timeout / 1000.0)) - { - transition_table_.signal(LocalizationSignal::GNSS_DATA_TIMEOUT); - } +void LocalizationManager::posePubTick() +{ + // Clear any expired timers + clearTimers(); - // Used in LocalizerMode::GNSS_WITH_NDT_INIT - // If the state is not Operational with good NDT, or already using GPS only, we reset the counter - if (transition_table_.getState() != LocalizationState::OPERATIONAL && - transition_table_.getState() != LocalizationState::DEGRADED_NO_LIDAR_FIX) - { - lidar_init_sequential_timesteps_counter_ = 0; - ROS_DEBUG_STREAM("posePubTick: Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); - } + // Evaluate NDT Frequency if we have started receiving messages + // This check provides protection against excessively long NDT computation times that do not trigger the callback + if (prev_ndt_stamp_) + { + double freq = computeFreq(rclcpp::Time(prev_ndt_stamp_.get(),timer_clock_type_), timer_factory_->now()); + if (freq <= config_.ndt_frequency_fault_threshold) + { + transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); + } + else if (freq <= config_.ndt_frequency_degraded_threshold) + { + transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); + } + } - // Publish current pose message if available - if (current_pose_){ - pose_pub_(*current_pose_); - } + // check if last gnss time stamp is older than threshold and send the corresponding signal + if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration(config_.gnss_data_timeout * 1e6)) + { + transition_table_.signal(LocalizationSignal::GNSS_DATA_TIMEOUT); + } - // Create and publish status report message - cav_msgs::LocalizationStatusReport msg = stateToMsg(transition_table_.getState(), ros::Time::now()); - state_pub_(msg); + // Used in LocalizerMode::GNSS_WITH_NDT_INIT + // If the state is not Operational with good NDT, or already using GPS only, we reset the counter + if (transition_table_.getState() != LocalizationState::OPERATIONAL && + transition_table_.getState() != LocalizationState::DEGRADED_NO_LIDAR_FIX) + { + lidar_init_sequential_timesteps_counter_ = 0; + RCLCPP_DEBUG_STREAM(logger_->get_logger(), "posePubTick: Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); + } + + // Publish current pose message if available + if (current_pose_){ + pose_pub_(*current_pose_); + } + // Create and publish status report message + carma_localization_msgs::msg::LocalizationStatusReport msg = stateToMsg(transition_table_.getState(), timer_factory_->now()); + state_pub_(msg); } LocalizationState LocalizationManager::getState() const @@ -274,4 +311,4 @@ LocalizationState LocalizationManager::getState() const return transition_table_.getState(); } -} // namespace localizer +} \ No newline at end of file diff --git a/localization_manager/src/LocalizationTransitionTable.cpp b/localization_manager/src/LocalizationTransitionTable.cpp index 98c04b8c11..77190ebfcc 100644 --- a/localization_manager/src/LocalizationTransitionTable.cpp +++ b/localization_manager/src/LocalizationTransitionTable.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -14,14 +14,15 @@ * the License. */ -#include -#include "localization_manager/LocalizationTransitionTable.h" -namespace localizer -{ -LocalizationTransitionTable::LocalizationTransitionTable(LocalizerMode mode) : mode_(mode) +#include "localization_manager/LocalizationTransitionTable.hpp" + + +namespace localization_manager { -} + +LocalizationTransitionTable::LocalizationTransitionTable(LocalizerMode mode) : mode_(mode) {} + LocalizationState LocalizationTransitionTable::getState() const { return state_; @@ -29,24 +30,22 @@ LocalizationState LocalizationTransitionTable::getState() const void LocalizationTransitionTable::logDebugSignal(LocalizationSignal signal) const { - ROS_DEBUG_STREAM("LocalizationTransitionTable received unsupported signal of " << signal << " while in state " - << state_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "LocalizationTransitionTable received unsupported signal of "<< signal <<"while in state"< +#include +#include + +namespace localization_manager +{ + namespace std_ph = std::placeholders; + + Node::Node(const rclcpp::NodeOptions &options) + :carma_ros2_utils::CarmaLifecycleNode(options) + + { + //Create initial config + config_ = LocalizationManagerConfig(); + + //Declare parameters + config_.fitness_score_degraded_threshold = declare_parameter("fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold); + config_.fitness_score_fault_threshold = declare_parameter("fitness_score_fault_threshold",config_.fitness_score_fault_threshold); + config_.ndt_frequency_degraded_threshold = declare_parameter("ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold); + config_.ndt_frequency_fault_threshold = declare_parameter("ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold); + config_.auto_initialization_timeout = declare_parameter("auto_initialization_timeout", config_.auto_initialization_timeout); + config_.gnss_only_operation_timeout = declare_parameter("gnss_only_operation_timeout", config_.gnss_only_operation_timeout); + config_.sequential_timesteps_until_gps_operation = declare_parameter("sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation); + config_.gnss_data_timeout = declare_parameter("gnss_data_timeout", config_.gnss_data_timeout); + config_.localization_mode = declare_parameter("localization_mode", config_.localization_mode); + config_.pose_pub_rate = declare_parameter("pose_pub_rate", config_.pose_pub_rate); + + } + + carma_ros2_utils::CallbackReturn Node::handle_on_configure(const rclcpp_lifecycle::State &) + { + // Reset config + config_ = LocalizationManagerConfig(); + + //Load parameters + get_parameter("fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold); + get_parameter("fitness_score_fault_threshold",config_.fitness_score_fault_threshold); + get_parameter("ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold); + get_parameter("ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold); + get_parameter("auto_initialization_timeout", config_.auto_initialization_timeout); + get_parameter("gnss_only_operation_timeout", config_.gnss_only_operation_timeout); + get_parameter("sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation); + get_parameter("gnss_data_timeout", config_.gnss_data_timeout); + get_parameter("localization_mode", config_.localization_mode); + get_parameter("pose_pub_rate", config_.pose_pub_rate); + + // Initialize worker object + manager_.reset(new LocalizationManager(std::bind(&Node::publishPoseStamped, this, std_ph::_1), + std::bind(&Node::publishStatus, this, std_ph::_1), + std::bind(&Node::publishManagedInitialPose, this, std_ph::_1), + config_, + get_node_logging_interface(), + std::make_unique(shared_from_this()) + )); + + // Setup subscribers + gnss_pose_sub_ = create_subscription("gnss_pose", 5, + std::bind(&LocalizationManager::gnssPoseCallback, manager_.get(), std_ph::_1)); + initialpose_sub_ = create_subscription("initialpose", 1, + std::bind(&LocalizationManager::initialPoseCallback, manager_.get(), std_ph::_1)); + + // Setup synchronized message_filters subscribers + rclcpp::QoS qos(5); + ndt_pose_sub_.subscribe(this, "ndt_pose", qos.get_rmw_qos_profile()); + ndt_score_sub_.subscribe(this,"ndt_stat", qos.get_rmw_qos_profile()); + + pose_stats_synchronizer_ = std::make_shared(ndt_pose_sub_, ndt_score_sub_, 5); + pose_stats_synchronizer_->registerCallback(std::bind(&Node::poseAndStatsCallback, this, std_ph::_1, std_ph::_2)); + + // system_alert_topic_ protected member of CarmaLifecycleNode + system_alert_sub_ = create_subscription(system_alert_topic_, 1, + std::bind(&LocalizationManager::systemAlertCallback, manager_.get(), std_ph::_1)); + // Setup publishers + pose_pub_ = create_publisher("selected_pose", 5); + state_pub_ = create_publisher("localization_status", 5); + + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too + rclcpp::PublisherOptions intra_proc_disabled; + intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object + + auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(1)); // 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. + managed_initial_pose_pub_ = create_publisher("managed_initialpose", pub_qos_transient_local, intra_proc_disabled); + + //Setup timer + pose_timer_ = create_timer(get_clock(), std::chrono::milliseconds(int(1/config_.pose_pub_rate * 1000)), + std::bind(&LocalizationManager::posePubTick, manager_.get())); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + + } + + void Node::publishPoseStamped(const geometry_msgs::msg::PoseStamped& msg) const + { + pose_pub_->publish(msg); + } + + void Node::publishStatus(const carma_localization_msgs::msg::LocalizationStatusReport& msg) const + { + state_pub_->publish(msg); + } + + void Node::publishManagedInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) const + { + managed_initial_pose_pub_->publish(msg); + } + + void Node::poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, + const autoware_msgs::msg::NDTStat::ConstPtr stats) + { + try + { + manager_->poseAndStatsCallback(pose, stats); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM(get_logger(), "Uncaught Exception in localization_manager. Exception: "<< e.what() ); + handle_primary_state_exception(e); + } + + + } +} // namespace localization_manager + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(localization_manager::Node) \ No newline at end of file diff --git a/localization_manager/src/localizer_node.cpp b/localization_manager/src/localizer_node.cpp deleted file mode 100644 index 712677315f..0000000000 --- a/localization_manager/src/localizer_node.cpp +++ /dev/null @@ -1,116 +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 "localization_manager/localizer_node.h" -#include - -namespace localizer -{ -namespace std_ph = std::placeholders; // Make alias for std placeholders to differentiate from boost. - -void Localizer::publishPoseStamped(const geometry_msgs::PoseStamped& msg) const -{ - pose_pub_.publish(msg); -} - -void Localizer::publishStatus(const cav_msgs::LocalizationStatusReport& msg) const -{ - state_pub_.publish(msg); -} - - -void Localizer::publishManagedInitialPose(const geometry_msgs::PoseWithCovarianceStamped& msg) const -{ - managed_initial_pose_pub_.publish(msg); -} - -void Localizer::poseAndStatsCallback(const geometry_msgs::PoseStampedConstPtr& pose, - const autoware_msgs::NDTStatConstPtr& stats) const -{ - try - { - manager_->poseAndStatsCallback(pose, stats); - } - catch (const std::exception& e) - { - ros::CARMANodeHandle::handleException(e); - } -} - -void Localizer::run() -{ - // initialize node handles - nh_ = ros::CARMANodeHandle(); - pnh_ = ros::CARMANodeHandle("~"); - // get params - LocalizationManagerConfig config; - - pnh_.param("pose_pub_rate", pose_pub_rate_, 10.0); - pnh_.param("fitness_score_degraded_threshold", config.fitness_score_degraded_threshold, - config.fitness_score_degraded_threshold); - pnh_.param("fitness_score_fault_threshold", config.fitness_score_fault_threshold, - config.fitness_score_fault_threshold); - pnh_.param("ndt_frequency_degraded_threshold", config.ndt_frequency_degraded_threshold, - config.ndt_frequency_degraded_threshold); - pnh_.param("ndt_frequency_fault_threshold", config.ndt_frequency_fault_threshold, - config.ndt_frequency_fault_threshold); - pnh_.param("auto_initialization_timeout", config.auto_initialization_timeout, - config.auto_initialization_timeout); - pnh_.param("gnss_only_operation_timeout", config.gnss_only_operation_timeout, - config.gnss_only_operation_timeout); - pnh_.param("sequential_timesteps_until_gps_operation", config.sequential_timesteps_until_gps_operation, - config.sequential_timesteps_until_gps_operation); - pnh_.param("gnss_data_timeout", config.gnss_data_timeout, - config.gnss_data_timeout); - - int localization_mode; - pnh_.param("localization_mode", localization_mode, 0); - config.localization_mode = static_cast(localization_mode); - - // Initialize worker object - manager_.reset(new LocalizationManager(std::bind(&Localizer::publishPoseStamped, this, std_ph::_1), - std::bind(&Localizer::publishStatus, this, std_ph::_1), - std::bind(&Localizer::publishManagedInitialPose, this, std_ph::_1), - config, - std::make_unique())); - - // initialize subscribers - gnss_pose_sub_ = nh_.subscribe("gnss_pose", 5, &LocalizationManager::gnssPoseCallback, manager_.get()); - initialpose_sub_ = nh_.subscribe("initialpose", 1, &LocalizationManager::initialPoseCallback, manager_.get()); - - // Setup synchronized message_filters subscribers - message_filters::Subscriber pose_sub(nh_, "ndt_pose", 5); - message_filters::Subscriber stats_sub(nh_, "ndt_stat", 5); - - PoseStatsSynchronizer pose_stats_synchronizer(PoseStatsSyncPolicy(5), pose_sub, stats_sub); - - pose_stats_synchronizer.registerCallback(boost::bind(&Localizer::poseAndStatsCallback, this, _1, _2)); - - ros::CARMANodeHandle::setSystemAlertCallback(std::bind(&LocalizationManager::systemAlertCallback, manager_.get(), std_ph::_1)); - - // initialize publishers - managed_initial_pose_pub_ = nh_.advertise("managed_initialpose", 1, true); - pose_pub_ = nh_.advertise("selected_pose", 5); - state_pub_ = nh_.advertise("localization_status", 1); - - // spin - pose_timer_ = nh_.createTimer( - ros::Duration(ros::Rate(pose_pub_rate_)), - &LocalizationManager::posePubTick, - manager_.get()); - ros::CARMANodeHandle::spin(); -} -} // namespace localizer diff --git a/localization_manager/src/main.cpp b/localization_manager/src/main.cpp index d591472ca3..776072d817 100644 --- a/localization_manager/src/main.cpp +++ b/localization_manager/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2021 LEIDOS. + * 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 @@ -14,13 +14,20 @@ * the License. */ -#include -#include "localization_manager/localizer_node.h" +#include +#include "localization_manager/localization_manager_node.hpp" -int main(int argc, char** argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "gnss_ndt_selector"); - localizer::Localizer node; - node.run(); + 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/localization_manager/test/TestLocalizationManager.cpp b/localization_manager/test/TestLocalizationManager.cpp index 5a1ae3ec3d..12c52b7b41 100644 --- a/localization_manager/test/TestLocalizationManager.cpp +++ b/localization_manager/test/TestLocalizationManager.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -14,325 +14,351 @@ * the License. */ + #include -#include +#include #include -#include -#include -#include -#include -#include "localization_manager/LocalizationManager.h" - - -namespace localizer -{ - -// Helper function returns the config that these tests were origionally meant to assume was the default -LocalizationManagerConfig getDefaultConfigForTest() { - LocalizationManagerConfig config; - config.fitness_score_degraded_threshold = 1.0; - config.fitness_score_fault_threshold = 2.0; - config.ndt_frequency_degraded_threshold = 9; - config.ndt_frequency_fault_threshold = 5; - config.auto_initialization_timeout = 30000; - config.gnss_only_operation_timeout = 6000; - config.gnss_data_timeout = 500; - config.localization_mode = LocalizerMode::NDT; - - return config; -} - -TEST(LocalizationManager, testConstructor) -{ - LocalizationManagerConfig config = getDefaultConfigForTest(); - LocalizationManager manager([](auto pose) {}, [](auto status) {}, [](auto pose_with_cov) {}, config, - std::make_unique()); -} - -TEST(LocalizationManager, testSpin) -{ - LocalizationManagerConfig config = getDefaultConfigForTest(); - - boost::optional status_msg; - LocalizationManager manager([](auto pose) {}, [&](auto status) { status_msg = status; }, [](auto pose_with_cov) {}, config, - std::make_unique()); - - ASSERT_FALSE(!!status_msg); - - ros::Time::setNow(ros::Time(1.0)); - - ros::TimerEvent e; - manager.posePubTick(e); - ASSERT_TRUE(!!status_msg); - - ASSERT_EQ(cav_msgs::LocalizationStatusReport::UNINITIALIZED, status_msg.get().status); - ASSERT_EQ(ros::Time::now().toSec(), status_msg.get().header.stamp.toSec()); -} - -TEST(LocalizationManager, testGNSSTimeout) -{ - LocalizationManagerConfig config = getDefaultConfigForTest(); - config.localization_mode = LocalizerMode::GNSS; - - boost::optional status_msg; - LocalizationManager manager([](auto pose) {}, [&](auto status) { status_msg = status; }, [](auto pose_with_cov) {}, config, - std::make_unique()); - - ASSERT_FALSE(!!status_msg); +#include +#include "localization_manager/LocalizationManager.hpp" - ros::Time::setNow(ros::Time(1.0)); +#include +#include +#include +#include "rclcpp_lifecycle/lifecycle_node.hpp" - geometry_msgs::PoseWithCovarianceStamped msg; - msg.header.seq = 1; - msg.header.stamp = ros::Time::now(); - geometry_msgs::PoseWithCovarianceStampedConstPtr msg_ptr(new geometry_msgs::PoseWithCovarianceStamped(msg)); - manager.initialPoseCallback(msg_ptr); +using carma_ros2_utils::timers::testing::TestTimer; +using carma_ros2_utils::timers::testing::TestTimerFactory; - geometry_msgs::PoseStamped msg_2; - msg_2.header.seq = 1; - msg_2.header.stamp = ros::Time::now(); - geometry_msgs::PoseStampedConstPtr msg_ptr_2(new geometry_msgs::PoseStamped(msg_2)); - manager.gnssPoseCallback(msg_ptr_2); - - - ros::TimerEvent e; - manager.posePubTick(e); - ASSERT_TRUE(!!status_msg); - - ASSERT_EQ(cav_msgs::LocalizationStatusReport::DEGRADED_NO_LIDAR_FIX, status_msg.get().status); - ASSERT_EQ(ros::Time::now().toSec(), status_msg.get().header.stamp.toSec()); - - ros::Time::setNow(ros::Time(1.1)); // Still no timeout - - manager.posePubTick(e); - ASSERT_TRUE(!!status_msg); - - ASSERT_EQ(cav_msgs::LocalizationStatusReport::DEGRADED_NO_LIDAR_FIX, status_msg.get().status); - ASSERT_EQ(ros::Time::now().toSec(), status_msg.get().header.stamp.toSec()); - - ros::Time::setNow(ros::Time(1.6)); // timout - - ASSERT_THROW(manager.posePubTick(e), std::runtime_error); -} - -TEST(LocalizationManager, testSignals) +namespace localization_manager { - LocalizationManagerConfig config = getDefaultConfigForTest(); - config.localization_mode = LocalizerMode::AUTO_WITH_TIMEOUT; - config.auto_initialization_timeout = 1000; - config.gnss_only_operation_timeout = 2000; - - boost::optional published_pose; - boost::optional published_initial_pose; + TEST(LocalizationManager, DISABLED_testConstructor) + { + // Using default values from LocalizationManagerConfig + LocalizationManagerConfig config; + + auto worker_node = rclcpp_lifecycle::LifecycleNode::make_shared("worker_node"); + worker_node->configure(); + worker_node->activate(); + + auto timer = std::make_unique(); + timer->buildTimer(0, rclcpp::Duration(0,0), [](){}, true, true); + boost::optional status_msg; + LocalizationManager manager([](auto pose) {}, [&](auto status) {status_msg = status;}, [](auto pose_with_cov) {}, config, + worker_node->get_node_logging_interface(), + std::move(timer)); + + } + + TEST(LocalizationManager, DISABLED_testSpin) + { + // Using default values from LocalizationManagerConfig + LocalizationManagerConfig config; + + auto worker_node = rclcpp_lifecycle::LifecycleNode::make_shared("worker_node"); + worker_node->configure(); + worker_node->activate(); + + auto timer = std::make_unique(); + // Initialize clock in timer + timer->buildTimer(0, rclcpp::Duration(0,0), [](){}, true, true); + timer->setNow(rclcpp::Time(1.0e9)); + rclcpp::Time time_now = timer->now(); + boost::optional status_msg; + LocalizationManager manager([](auto pose) {}, [&](auto status) {status_msg = status;}, [](auto pose_with_cov) {}, config, + worker_node->get_node_logging_interface(), + std::move(timer)); + + ASSERT_FALSE(!!status_msg); + + manager.posePubTick(); + ASSERT_TRUE(!!status_msg); + + ASSERT_EQ(carma_localization_msgs::msg::LocalizationStatusReport::UNINITIALIZED, status_msg.get().status); + ASSERT_EQ(time_now.seconds(), status_msg.get().header.stamp.sec); + } + + TEST(LocalizationManager, DISABLED_testGNSSTimeout) + { + // Using default values from LocalizationManagerConfig + LocalizationManagerConfig config; + config.localization_mode = LocalizerMode::GNSS; + + auto worker_node = rclcpp_lifecycle::LifecycleNode::make_shared("worker_node"); + worker_node->configure(); + worker_node->activate(); + + carma_ros2_utils::timers::testing::TestTimerFactory timer; + // Initialize clock in timer + timer.buildTimer(0, rclcpp::Duration(0,0), [](){}, true, true); + + boost::optional status_msg; + LocalizationManager manager([](auto pose) {}, [&](auto status) {status_msg = status;}, [](auto pose_with_cov) {}, config, + worker_node->get_node_logging_interface(), + std::make_unique(timer)); + + ASSERT_FALSE(!!status_msg); + + timer.setNow(rclcpp::Time(1e9, worker_node->now().get_clock_type())); + + geometry_msgs::msg::PoseWithCovarianceStamped msg; + msg.header.stamp = timer.now(); + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr(new geometry_msgs::msg::PoseWithCovarianceStamped(msg)); + manager.initialPoseCallback(msg_ptr); + + geometry_msgs::msg::PoseStamped msg_2; + msg_2.header.stamp = timer.now(); + geometry_msgs::msg::PoseStamped::SharedPtr msg_ptr_2(new geometry_msgs::msg::PoseStamped(msg_2)); + manager.gnssPoseCallback(msg_ptr_2); + + manager.posePubTick(); + ASSERT_TRUE(!!status_msg); + + ASSERT_EQ(carma_localization_msgs::msg::LocalizationStatusReport::DEGRADED_NO_LIDAR_FIX, status_msg->status); + ASSERT_EQ(timer.now().seconds(), rclcpp::Time(status_msg.get().header.stamp).seconds()); + + + timer.setNow(rclcpp::Time(1, 1e8, worker_node->now().get_clock_type())); // Still no timeout + manager.posePubTick(); + ASSERT_TRUE(!!status_msg); + + ASSERT_EQ(carma_localization_msgs::msg::LocalizationStatusReport::DEGRADED_NO_LIDAR_FIX, status_msg->status); + ASSERT_EQ(timer.now().seconds(), rclcpp::Time(status_msg.get().header.stamp).seconds()); + + timer.setNow(rclcpp::Time(1, 6e8, worker_node->now().get_clock_type())); + ASSERT_THROW(manager.posePubTick(), std::runtime_error); + + } + + TEST(LocalizationManager, testSignals) + { + LocalizationManagerConfig config; + config.localization_mode = LocalizerMode::AUTO_WITH_TIMEOUT; + config.auto_initialization_timeout = 1000; + config.gnss_only_operation_timeout = 2000; + config.fitness_score_degraded_threshold = 1.0; + + config.fitness_score_fault_threshold = 2.0; + config.ndt_frequency_degraded_threshold = 9; + config.ndt_frequency_fault_threshold = 5; + config.gnss_data_timeout = 500; + + boost::optional published_pose; + boost::optional published_initial_pose; + + auto worker_node = rclcpp_lifecycle::LifecycleNode::make_shared("worker_node"); + worker_node->configure(); + worker_node->activate(); + + carma_ros2_utils::timers::testing::TestTimerFactory timer; + // Initialize clock in timer + timer.buildTimer(0, rclcpp::Duration(0,0), [](){}, true, true); + + LocalizationManager manager([&](auto pose) {published_pose = pose;}, [&](auto status) {}, [&](auto initial) { published_initial_pose = initial; }, config, + worker_node->get_node_logging_interface(), + std::make_unique(timer)); - LocalizationManager manager([&](auto pose) { published_pose = pose; }, - [](auto status) {}, [&](auto initial) { published_initial_pose = initial; }, config, - std::make_unique()); - ros::Time::setNow(ros::Time(1.0)); + timer.setNow(rclcpp::Time(1e9, worker_node->now().get_clock_type())); + + ASSERT_EQ(LocalizationState::UNINITIALIZED, manager.getState()); - ASSERT_EQ(LocalizationState::UNINITIALIZED, manager.getState()); + geometry_msgs::msg::PoseWithCovarianceStamped msg; + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg_ptr(new geometry_msgs::msg::PoseWithCovarianceStamped(msg)); + manager.initialPoseCallback(msg_ptr); - geometry_msgs::PoseWithCovarianceStamped msg; - msg.header.seq = 1; - geometry_msgs::PoseWithCovarianceStampedConstPtr msg_ptr(new geometry_msgs::PoseWithCovarianceStamped(msg)); - manager.initialPoseCallback(msg_ptr); + ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); + ASSERT_TRUE(!!published_initial_pose); + + timer.setNow(rclcpp::Time(2, 1e8, RCL_SYSTEM_TIME)); - ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); - ASSERT_TRUE(!!published_initial_pose); - ASSERT_EQ(msg.header.seq, published_initial_pose->header.seq); + auto period = std::chrono::milliseconds(2000); + std::this_thread::sleep_for(period); - ros::Time::setNow(ros::Time(2.1)); + ASSERT_EQ(LocalizationState::AWAIT_MANUAL_INITIALIZATION, manager.getState()); - auto period = std::chrono::milliseconds(2000); - std::this_thread::sleep_for(period); + timer.setNow(rclcpp::Time(3, 1e8, RCL_SYSTEM_TIME)); + manager.initialPoseCallback(msg_ptr); - ASSERT_EQ(LocalizationState::AWAIT_MANUAL_INITIALIZATION, manager.getState()); + ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); - ros::Time::setNow(ros::Time(3.1)); + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.stamp = timer.now(); + geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr(new geometry_msgs::msg::PoseStamped(pose_msg)); - manager.initialPoseCallback(msg_ptr); + autoware_msgs::msg::NDTStat stat_msg; + stat_msg.score = 0.1; + autoware_msgs::msg::NDTStat::SharedPtr stat_msg_ptr(new autoware_msgs::msg::NDTStat(stat_msg)); - ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - geometry_msgs::PoseStamped pose_msg; - pose_msg.header.stamp = ros::Time::now(); - geometry_msgs::PoseStampedConstPtr pose_msg_ptr(new geometry_msgs::PoseStamped(pose_msg)); + ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); + manager.posePubTick(); + ASSERT_TRUE(!!published_pose); - autoware_msgs::NDTStat stat_msg; - stat_msg.score = 0.1; - autoware_msgs::NDTStatConstPtr stat_msg_ptr(new autoware_msgs::NDTStat(stat_msg)); + timer.setNow(rclcpp::Time(3, 2e8, RCL_SYSTEM_TIME)); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr (new geometry_msgs::msg::PoseStamped(pose_msg)); + stat_msg.score = 1.1; + stat_msg_ptr = autoware_msgs::msg::NDTStat::SharedPtr (new autoware_msgs::msg::NDTStat(stat_msg)); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); - ros::TimerEvent e; - manager.posePubTick(e); - ASSERT_TRUE(!!published_pose); + ASSERT_EQ(LocalizationState::DEGRADED, manager.getState()); - ros::Time::setNow(ros::Time(3.2)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); - stat_msg.score = 1.1; - stat_msg_ptr = autoware_msgs::NDTStatConstPtr(new autoware_msgs::NDTStat(stat_msg)); + timer.setNow(rclcpp::Time(3,3e8, RCL_SYSTEM_TIME)); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); + stat_msg.score = 0.1; + stat_msg_ptr = autoware_msgs::msg::NDTStat::SharedPtr(new autoware_msgs::msg::NDTStat(stat_msg)); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_EQ(LocalizationState::DEGRADED, manager.getState()); + ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); - ros::Time::setNow(ros::Time(3.3)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); - stat_msg.score = 0.1; - stat_msg_ptr = autoware_msgs::NDTStatConstPtr(new autoware_msgs::NDTStat(stat_msg)); + timer.setNow(rclcpp::Time(3,415e6, RCL_SYSTEM_TIME)); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); + ASSERT_EQ(LocalizationState::DEGRADED, manager.getState()); - ros::Time::setNow(ros::Time(3.415)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); + timer.setNow(rclcpp::Time(3,5e8, RCL_SYSTEM_TIME)); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_EQ(LocalizationState::DEGRADED, manager.getState()); + ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); - ros::Time::setNow(ros::Time(3.5)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); + timer.setNow(rclcpp::Time(3,7e8, RCL_SYSTEM_TIME)); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); + ASSERT_EQ(LocalizationState::DEGRADED_NO_LIDAR_FIX, manager.getState()); - ros::Time::setNow(ros::Time(3.7)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); + published_pose = boost::none; + published_initial_pose = boost::none; - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_EQ(LocalizationState::DEGRADED_NO_LIDAR_FIX, manager.getState()); + ASSERT_FALSE(!!published_pose); - published_pose = boost::none; - published_initial_pose = boost::none; + manager.gnssPoseCallback(pose_msg_ptr); + manager.posePubTick(); + ASSERT_TRUE(!!published_pose); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + timer.setNow(rclcpp::Time(3,7e8, RCL_SYSTEM_TIME)); - ASSERT_FALSE(!!published_pose); + manager.initialPoseCallback(msg_ptr); - manager.gnssPoseCallback(pose_msg_ptr); - manager.posePubTick(e); - ASSERT_TRUE(!!published_pose); + ASSERT_TRUE(!!published_initial_pose); + ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); - ros::Time::setNow(ros::Time(3.7)); + timer.setNow(rclcpp::Time(3,8e8, RCL_SYSTEM_TIME)); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - manager.initialPoseCallback(msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_TRUE(!!published_initial_pose); + ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); - ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ros::Time::setNow(ros::Time(3.8)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); + ASSERT_EQ(LocalizationState::DEGRADED_NO_LIDAR_FIX, manager.getState()); + manager.gnssPoseCallback(pose_msg_ptr); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + timer.setNow(rclcpp::Time(5,9e8, RCL_SYSTEM_TIME)); - ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); + std::this_thread::sleep_for(period); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + ASSERT_EQ(LocalizationState::AWAIT_MANUAL_INITIALIZATION, manager.getState()); - ASSERT_EQ(LocalizationState::DEGRADED_NO_LIDAR_FIX, manager.getState()); - manager.gnssPoseCallback(pose_msg_ptr); + timer.setNow(rclcpp::Time(6,0, RCL_SYSTEM_TIME)); - ros::Time::setNow(ros::Time(5.9)); + manager.initialPoseCallback(msg_ptr); - std::this_thread::sleep_for(period); + ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); - ASSERT_EQ(LocalizationState::AWAIT_MANUAL_INITIALIZATION, manager.getState()); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - ros::Time::setNow(ros::Time(6.0)); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - manager.initialPoseCallback(msg_ptr); + timer.setNow(rclcpp::Time(6, 1e8, RCL_SYSTEM_TIME)); - ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); - ros::Time::setNow(ros::Time(6.1)); + timer.setNow(rclcpp::Time(6,215e6, RCL_SYSTEM_TIME)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); + manager.posePubTick(); + ASSERT_EQ(LocalizationState::DEGRADED, manager.getState()); - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + manager.initialPoseCallback(msg_ptr); + ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); - ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); + timer.setNow(rclcpp::Time(6,3e8)); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - ros::Time::setNow(ros::Time(6.215)); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - manager.posePubTick(e); + timer.setNow(rclcpp::Time(6,4e8, RCL_SYSTEM_TIME)); - ASSERT_EQ(LocalizationState::DEGRADED, manager.getState()); + pose_msg.header.stamp = timer.now(); + pose_msg_ptr = geometry_msgs::msg::PoseStamped::SharedPtr(new geometry_msgs::msg::PoseStamped(pose_msg)); - manager.initialPoseCallback(msg_ptr); + manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); + ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); - ros::Time::setNow(ros::Time(6.3)); - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); - - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); - - ros::Time::setNow(ros::Time(6.4)); - - pose_msg.header.stamp = ros::Time::now(); - pose_msg_ptr = geometry_msgs::PoseStampedConstPtr(new geometry_msgs::PoseStamped(pose_msg)); - - manager.poseAndStatsCallback(pose_msg_ptr, stat_msg_ptr); + timer.setNow(rclcpp::Time(6,6e8, RCL_SYSTEM_TIME)); + + manager.gnssPoseCallback(pose_msg_ptr); + manager.posePubTick(); - ASSERT_EQ(LocalizationState::OPERATIONAL, manager.getState()); + ASSERT_EQ(LocalizationState::DEGRADED_NO_LIDAR_FIX, manager.getState()); - ros::Time::setNow(ros::Time(6.6)); - - manager.gnssPoseCallback(pose_msg_ptr); - manager.posePubTick(e); + } - ASSERT_EQ(LocalizationState::DEGRADED_NO_LIDAR_FIX, manager.getState()); - -} + TEST(LocalizationManager, DISABLED_testGNSSCorrection) + { + LocalizationManagerConfig config; + config.localization_mode = static_cast(LocalizerMode::AUTO_WITHOUT_TIMEOUT); + config.auto_initialization_timeout = 1000; + config.gnss_only_operation_timeout = 2000; -TEST(LocalizationManager, testGNSSCorrection) -{ - LocalizationManagerConfig config = getDefaultConfigForTest(); - config.localization_mode = LocalizerMode::AUTO_WITHOUT_TIMEOUT; - config.auto_initialization_timeout = 1000; - config.gnss_only_operation_timeout = 2000; + boost::optional published_pose; + boost::optional published_initial_pose; - boost::optional published_pose; - boost::optional published_initial_pose; + auto worker_node = rclcpp_lifecycle::LifecycleNode::make_shared("worker_node"); + worker_node->configure(); + worker_node->activate(); - LocalizationManager manager([&](auto pose) { published_pose = pose; }, - [](auto status) {}, [&](auto initial) { published_initial_pose = initial; }, config, - std::make_unique()); + carma_ros2_utils::timers::testing::TestTimerFactory timer; + // Initialize clock in timer + timer.buildTimer(0, rclcpp::Duration(0,0), [](){}, true, true); + LocalizationManager manager([&](auto pose) {published_pose = pose;}, [&](auto status) {}, [&](auto initial) { published_initial_pose = initial; }, config, + worker_node->get_node_logging_interface(), + std::make_unique(timer)); - ros::Time::setNow(ros::Time(1.0)); + timer.setNow(rclcpp::Time(1.0e9, RCL_SYSTEM_TIME)); - ASSERT_EQ(LocalizationState::UNINITIALIZED, manager.getState()); + ASSERT_EQ(LocalizationState::UNINITIALIZED, manager.getState()); - geometry_msgs::PoseStamped msg; - msg.header.seq = 1; - geometry_msgs::PoseStampedConstPtr msg_ptr(new geometry_msgs::PoseStamped(msg)); - manager.gnssPoseCallback(msg_ptr); + geometry_msgs::msg::PoseStamped msg; + geometry_msgs::msg::PoseStamped::SharedPtr msg_ptr(new geometry_msgs::msg::PoseStamped(msg)); + manager.gnssPoseCallback(msg_ptr); - ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); - ASSERT_TRUE(!!published_initial_pose); - ASSERT_EQ(msg.header.seq, published_initial_pose->header.seq); + ASSERT_EQ(LocalizationState::INITIALIZING, manager.getState()); + ASSERT_TRUE(!!published_initial_pose); + } } - -}; // localizer \ No newline at end of file diff --git a/localization_manager/test/TestLocalizationTransitionTable.cpp b/localization_manager/test/TestLocalizationTransitionTable.cpp index cb75e11cf4..8cbc979842 100644 --- a/localization_manager/test/TestLocalizationTransitionTable.cpp +++ b/localization_manager/test/TestLocalizationTransitionTable.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -15,13 +15,13 @@ */ #include -#include +#include #include -#include "localization_manager/LocalizationTransitionTable.h" +#include "localization_manager/LocalizationTransitionTable.hpp" -using namespace localizer; - -/** +namespace localization_manager +{ + /** * \brief Helper function to assert that the provided list of signals do not change the current state of the provided * transition table */ @@ -116,6 +116,7 @@ TEST(LocalizationTransitionTable, testTransitionsGNSSMode) }); } + TEST(LocalizationTransitionTable, testTransitionsNDTMode) { // Test NDT Mode @@ -335,3 +336,5 @@ TEST(LocalizationTransitionTable, testTransitionsAUTOMode) ltt.signal(LocalizationSignal::LIDAR_SENSOR_FAILURE); ASSERT_EQ(LocalizationState::DEGRADED_NO_LIDAR_FIX, ltt.getState()); } + +} diff --git a/localization_manager/test/TestLocalizationTypes.cpp b/localization_manager/test/TestLocalizationTypes.cpp index 1fd4c4b9cd..e1db5294b6 100644 --- a/localization_manager/test/TestLocalizationTypes.cpp +++ b/localization_manager/test/TestLocalizationTypes.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -16,9 +16,10 @@ #include #include -#include "localization_manager/LocalizationTypes.h" +#include "localization_manager/LocalizationTypes.hpp" -using namespace localizer; +namespace localization_manager +{ TEST(LocalizationState, testStream) { @@ -50,38 +51,38 @@ TEST(LocalizationState, testStream) TEST(LocalizationState, testStateToMsg) { - cav_msgs::LocalizationStatusReport msg; - ros::Time stamp = ros::Time(1.0); + carma_localization_msgs::msg::LocalizationStatusReport msg; + rclcpp::Time stamp = rclcpp::Time(1.0,0, RCL_SYSTEM_TIME); LocalizationState state = LocalizationState::UNINITIALIZED; msg = stateToMsg(state, stamp); - ASSERT_EQ(msg.status, cav_msgs::LocalizationStatusReport::UNINITIALIZED); - ASSERT_EQ(msg.header.stamp.toSec(), stamp.toSec()); + ASSERT_EQ(msg.status, carma_localization_msgs::msg::LocalizationStatusReport::UNINITIALIZED); + ASSERT_EQ(msg.header.stamp.sec, stamp.seconds()); state = LocalizationState::INITIALIZING; msg = stateToMsg(state, stamp); - ASSERT_EQ(msg.status, cav_msgs::LocalizationStatusReport::INITIALIZING); - ASSERT_EQ(msg.header.stamp.toSec(), stamp.toSec()); + ASSERT_EQ(msg.status, carma_localization_msgs::msg::LocalizationStatusReport::INITIALIZING); + ASSERT_EQ(msg.header.stamp.sec, stamp.seconds()); state = LocalizationState::OPERATIONAL; msg = stateToMsg(state, stamp); - ASSERT_EQ(msg.status, cav_msgs::LocalizationStatusReport::OPERATIONAL); - ASSERT_EQ(msg.header.stamp.toSec(), stamp.toSec()); + ASSERT_EQ(msg.status, carma_localization_msgs::msg::LocalizationStatusReport::OPERATIONAL); + ASSERT_EQ(msg.header.stamp.sec, stamp.seconds()); state = LocalizationState::DEGRADED; msg = stateToMsg(state, stamp); - ASSERT_EQ(msg.status, cav_msgs::LocalizationStatusReport::DEGRADED); - ASSERT_EQ(msg.header.stamp.toSec(), stamp.toSec()); + ASSERT_EQ(msg.status, carma_localization_msgs::msg::LocalizationStatusReport::DEGRADED); + ASSERT_EQ(msg.header.stamp.sec, stamp.seconds()); state = LocalizationState::DEGRADED_NO_LIDAR_FIX; msg = stateToMsg(state, stamp); - ASSERT_EQ(msg.status, cav_msgs::LocalizationStatusReport::DEGRADED_NO_LIDAR_FIX); - ASSERT_EQ(msg.header.stamp.toSec(), stamp.toSec()); + ASSERT_EQ(msg.status, carma_localization_msgs::msg::LocalizationStatusReport::DEGRADED_NO_LIDAR_FIX); + ASSERT_EQ(msg.header.stamp.sec, stamp.seconds()); state = LocalizationState::AWAIT_MANUAL_INITIALIZATION; msg = stateToMsg(state, stamp); - ASSERT_EQ(msg.status, cav_msgs::LocalizationStatusReport::AWAIT_MANUAL_INITIALIZATION); - ASSERT_EQ(msg.header.stamp.toSec(), stamp.toSec()); + ASSERT_EQ(msg.status, carma_localization_msgs::msg::LocalizationStatusReport::AWAIT_MANUAL_INITIALIZATION); + ASSERT_EQ(msg.header.stamp.sec, stamp.seconds()); } TEST(LocalizationSignal, testStream) @@ -130,4 +131,7 @@ TEST(LocalizerMode, testStream) output << LocalizerMode::AUTO_WITHOUT_TIMEOUT; ASSERT_EQ("AUTO_WITHOUT_TIMEOUT", output.str()); output.str(std::string()); -} \ No newline at end of file +} + + +} diff --git a/localization_manager/test/TestMain.cpp b/localization_manager/test/TestMain.cpp index 7484b18f2c..c951713c27 100644 --- a/localization_manager/test/TestMain.cpp +++ b/localization_manager/test/TestMain.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -15,12 +15,25 @@ */ #include -#include +#include +#include +#include +#include + +#include "localization_manager/localization_manager_node.hpp" // Run all the tests int main(int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::Time::init(); - return RUN_ALL_TESTS(); + ::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 From 8846a4044d79eead5b6ad55249f6df74fc380e89 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Tue, 23 Aug 2022 08:18:27 -0700 Subject: [PATCH 080/165] Add README --- light_controlled_intersection_tactical_plugin/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/light_controlled_intersection_tactical_plugin/README.md b/light_controlled_intersection_tactical_plugin/README.md index f194feb521..c541e23e7a 100644 --- a/light_controlled_intersection_tactical_plugin/README.md +++ b/light_controlled_intersection_tactical_plugin/README.md @@ -1,3 +1,3 @@ # light_controlled_intersection_tactical_plugin -TODO for USER: Add description of package and link to confluence documentation. +The light_controlled_intersection_tactical_plugin is responsible for generating a trajectory plan with optimized vehicle speeds when traversing a light controlled intersection. This trajectory plan is generated using maneuvers received from the light_controlled_intersection_strategic_plugin. \ No newline at end of file From 2880879199181516d1ecea7cac2627d266aff0d5 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Tue, 23 Aug 2022 08:23:32 -0700 Subject: [PATCH 081/165] Remove global accel limits from dynamic updates --- .../light_controlled_intersection_tactical_plugin_node.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index 8321eb5814..bed5131e0e 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -62,10 +62,7 @@ namespace light_controlled_intersection_tactical_plugin {"stop_line_buffer", config_.stop_line_buffer}, {"minimum_speed", config_.minimum_speed}, {"algorithm_evaluation_distance", config_.algorithm_evaluation_distance}, - {"algorithm_evaluation_period", config_.algorithm_evaluation_period}, - {"vehicle_lateral_accel_limit", config_.lateral_accel_limit}, - {"vehicle_acceleration_limit", config_.vehicle_accel_limit}, - {"vehicle_deceleration_limit", config_.vehicle_decel_limit}}, parameters); + {"algorithm_evaluation_period", config_.algorithm_evaluation_period}}, parameters); // Global acceleration limits not allowed to dynamically update auto error_2 = update_params( {{"default_downsample_ratio", config_.default_downsample_ratio}, {"turn_downsample_ratio", config_.turn_downsample_ratio}, From b1263f747900c1b5cb524c120b4d71bcf35acd33 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Tue, 23 Aug 2022 10:29:14 -0700 Subject: [PATCH 082/165] Make class functions camelCase --- ...ontrolled_intersection_tactical_plugin.hpp | 13 ++++++++---- ...ontrolled_intersection_tactical_plugin.cpp | 20 ++++++++++++------- ...lled_intersection_tactical_plugin_node.cpp | 7 ++++++- 3 files changed, 28 insertions(+), 12 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp index f9e682217d..d38cd16fea 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp @@ -144,7 +144,7 @@ namespace light_controlled_intersection_tactical_plugin * NOTE: Cruising speed profile is applied (case 1) if speed before deceleration is higher than speed limit. Otherwise Case 2. * NOTE: when applying the speed profile, the function ignores buffer points beyond start_dist and end_dist. Internally uses: config_.back_distance and speed_limit_ */ - void apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp); /** @@ -156,7 +156,7 @@ namespace light_controlled_intersection_tactical_plugin * These points must be in the same lane as the vehicle and must extend in front of it though it is fine if they also extend behind it. * */ - void apply_optimized_target_speed_profile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); + void applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds); /** * \brief Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types (by converting it to LANE_FOLLOW) @@ -171,7 +171,7 @@ namespace light_controlled_intersection_tactical_plugin * \return A vector of point speed pair struct which contains geometry points as basicpoint::lanelet2d and speed as a double for the maneuver * NOTE: This function is a slightly modified version of the same function in basic_autonomy library and currently only plans for the first maneuver */ - std::vector create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, + std::vector createGeometryProfile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config); @@ -199,10 +199,15 @@ namespace light_controlled_intersection_tactical_plugin * \param req The service request * \param resp The service response */ - void plan_trajectory_cb( + void planTrajectoryCB( carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); + /** + * \brief Setter function to set a new config for this object + * \param config The new config to be used by this object + */ + void setConfig(const Config& config); }; } // light_controlled_intersection_tactical_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 6d7e590f69..754fe498b2 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -25,7 +25,7 @@ namespace light_controlled_intersection_tactical_plugin { } - void LightControlledIntersectionTacticalPlugin::plan_trajectory_cb( + void LightControlledIntersectionTacticalPlugin::planTrajectoryCB( carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { @@ -104,11 +104,11 @@ namespace light_controlled_intersection_tactical_plugin // Create curve-fitting compatible trajectories (with extra back and front attached points) with raw speed limits from maneuver - auto points_and_target_speeds = create_geometry_profile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance), + auto points_and_target_speeds = createGeometryProfile(maneuver_plan, std::max((double)0, current_downtrack_ - config_.back_distance), wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config); // Change raw speed limit values to target speeds specified by the algorithm - apply_optimized_target_speed_profile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel, points_and_target_speeds); + applyOptimizedTargetSpeedProfile(maneuver_plan.front(), req->vehicle_state.longitudinal_vel, points_and_target_speeds); RCLCPP_DEBUG_STREAM(logger_->get_logger(), "points_and_target_speeds: " << points_and_target_speeds.size()); @@ -212,7 +212,7 @@ namespace light_controlled_intersection_tactical_plugin return; } - void LightControlledIntersectionTacticalPlugin::apply_trajectory_smoothing_algorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, + void LightControlledIntersectionTacticalPlugin::applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr& wm, std::vector& points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp) { if (points_and_target_speeds.empty()) @@ -299,7 +299,7 @@ namespace light_controlled_intersection_tactical_plugin } } - void LightControlledIntersectionTacticalPlugin::apply_optimized_target_speed_profile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds) + void LightControlledIntersectionTacticalPlugin::applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver& maneuver, const double starting_speed, std::vector& points_and_target_speeds) { if(GET_MANEUVER_PROPERTY(maneuver, parameters.float_valued_meta_data).size() < 9 || GET_MANEUVER_PROPERTY(maneuver, parameters.int_valued_meta_data).size() < 2 ){ @@ -327,7 +327,7 @@ namespace light_controlled_intersection_tactical_plugin double entry_dist = ending_downtrack - starting_downtrack; // change speed profile depending on algorithm case starting from maneuver start_dist - apply_trajectory_smoothing_algorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, + applyTrajectorySmoothingAlgorithm(wm_, points_and_target_speeds, starting_downtrack, entry_dist, starting_speed, departure_speed, tsp); } @@ -344,7 +344,7 @@ namespace light_controlled_intersection_tactical_plugin } } - std::vector LightControlledIntersectionTacticalPlugin::create_geometry_profile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, + std::vector LightControlledIntersectionTacticalPlugin::createGeometryProfile(const std::vector &maneuvers, double max_starting_downtrack,const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,const carma_planning_msgs::msg::VehicleState& state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config) { @@ -386,4 +386,10 @@ namespace light_controlled_intersection_tactical_plugin return points_and_target_speeds; } + void LightControlledIntersectionTacticalPlugin::setConfig(const Config& config) + { + config_ = config; + } + + } // light_controlled_intersection_tactical_plugin \ No newline at end of file diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp index bed5131e0e..8a4d3f60a0 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin_node.cpp @@ -69,6 +69,11 @@ namespace light_controlled_intersection_tactical_plugin {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size}, {"speed_moving_average_window_size", config_.speed_moving_average_window_size}}, parameters); + if (worker_) + { + worker_->setConfig(config_); + } + rcl_interfaces::msg::SetParametersResult result; result.successful = !error && !error_2; @@ -134,7 +139,7 @@ namespace light_controlled_intersection_tactical_plugin carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) { - worker_->plan_trajectory_cb(req, resp); + worker_->planTrajectoryCB(req, resp); } } // light_controlled_intersection_tactical_plugin From b41fdbe1e3db1c846114db969b5a8d16258e0124 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Tue, 23 Aug 2022 16:14:28 -0400 Subject: [PATCH 083/165] Assorted fixes for plugins (#1902) This PR makes some assorted fixes to recently ported ROS2 plugins Fixes: Updated plugin discovery version of inlane-cruising to match package version (4.0) Make global parameters (which can't exist in ros2) local in inlanecruising, stop and wait, and route_following Remove duplicate PlanManeuvers service from route_following which was already provided by base class. Use valid world model instance in stop and wait plugin. Pointer was previously not initialized Add dynamic parameter update method to stop and wait plugin Related Issue Supports #1899 Co-authored-by: Misheel Bayartsengel --- .../src/inlanecruising_plugin_node.cpp | 15 +++---- .../include/route_following_plugin.hpp | 3 -- .../src/route_following_plugin.cpp | 18 ++++----- .../include/stop_and_wait_node.hpp | 7 +++- .../src/stop_and_wait_plugin_node.cpp | 40 +++++++++++++++++-- 5 files changed, 56 insertions(+), 27 deletions(-) diff --git a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp index f8e4c788fd..86ef0d129c 100644 --- a/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp +++ b/inlanecruising_plugin/src/inlanecruising_plugin_node.cpp @@ -23,7 +23,7 @@ namespace inlanecruising_plugin InLaneCruisingPluginNode::InLaneCruisingPluginNode(const rclcpp::NodeOptions &options) : carma_guidance_plugins::TacticalPlugin(options), plugin_name_(get_plugin_name_and_ns()), - version_id_("v1.0"), + version_id_("v4.0"), config_(InLaneCruisingPluginConfig()) { // Declare parameters @@ -38,8 +38,8 @@ namespace inlanecruising_plugin config_.speed_moving_average_window_size = declare_parameter("speed_moving_average_window_size", config_.speed_moving_average_window_size); config_.curvature_moving_average_window_size = declare_parameter("curvature_moving_average_window_size", config_.curvature_moving_average_window_size); config_.buffer_ending_downtrack = declare_parameter("buffer_ending_downtrack", config_.buffer_ending_downtrack); - config_.max_accel = declare_parameter("/vehicle_acceleration_limit", config_.max_accel); - config_.lateral_accel_limit = declare_parameter("/vehicle_lateral_accel_limit", config_.lateral_accel_limit); + config_.max_accel = declare_parameter("vehicle_acceleration_limit", config_.max_accel); + config_.lateral_accel_limit = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); } @@ -60,8 +60,8 @@ namespace inlanecruising_plugin get_parameter("speed_moving_average_window_size", config_.speed_moving_average_window_size); get_parameter("curvature_moving_average_window_size", config_.curvature_moving_average_window_size); get_parameter("buffer_ending_downtrack", config_.buffer_ending_downtrack); - get_parameter("/vehicle_acceleration_limit", config_.max_accel); - get_parameter("/vehicle_lateral_accel_limit", config_.lateral_accel_limit); + get_parameter("vehicle_acceleration_limit", config_.max_accel); + get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); get_parameter("enable_object_avoidance", config_.enable_object_avoidance); @@ -104,10 +104,7 @@ namespace inlanecruising_plugin {"max_accel_multiplier", config_.max_accel_multiplier}, {"lat_accel_multiplier", config_.lat_accel_multiplier}, {"back_distance", config_.back_distance}, - {"buffer_ending_downtrack", config_.buffer_ending_downtrack}, - {"/vehicle_acceleration_limit", config_.max_accel}, - {"/vehicle_lateral_accel_limit", config_.lateral_accel_limit}, - {"buffer_ending_downtrack", config_.buffer_ending_downtrack}}, parameters); + {"buffer_ending_downtrack", config_.buffer_ending_downtrack}}, parameters); // Global acceleration limits not allowed to dynamically update auto error_bool = update_params({ {"enable_object_avoidance", config_.enable_object_avoidance}}, parameters); diff --git a/route_following_plugin/include/route_following_plugin.hpp b/route_following_plugin/include/route_following_plugin.hpp index 7f110a2e6d..0096757614 100644 --- a/route_following_plugin/include/route_following_plugin.hpp +++ b/route_following_plugin/include/route_following_plugin.hpp @@ -240,9 +240,6 @@ namespace route_following_plugin // Publishers carma_ros2_utils::PubPtr upcoming_lane_change_status_pub_; - // Service Servers - carma_ros2_utils::ServicePtr plan_maneuver_srv_; - // unordered set of all the lanelet ids in shortest path std::unordered_set shortest_path_set_; diff --git a/route_following_plugin/src/route_following_plugin.cpp b/route_following_plugin/src/route_following_plugin.cpp index c479f5b17c..5dd415a80c 100644 --- a/route_following_plugin/src/route_following_plugin.cpp +++ b/route_following_plugin/src/route_following_plugin.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace route_following_plugin { @@ -92,18 +93,15 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id config_.stop_and_wait_plugin_ = declare_parameter("stop_and_wait_plugin", config_.stop_and_wait_plugin_); config_.lanefollow_planning_tactical_plugin_ = declare_parameter("lane_following_plugin", config_.lanefollow_planning_tactical_plugin_); config_.route_end_point_buffer_ = declare_parameter("/guidance/route/destination_downtrack_range", config_.route_end_point_buffer_); - config_.accel_limit_ = declare_parameter("/vehicle_acceleration_limit", config_.accel_limit_); - config_.lateral_accel_limit_ = declare_parameter("/vehicle_lateral_accel_limit", config_.lateral_accel_limit_); + config_.accel_limit_ = declare_parameter("vehicle_acceleration_limit", config_.accel_limit_); + config_.lateral_accel_limit_ = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit_); config_.stopping_accel_limit_multiplier_ = declare_parameter("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_); config_.min_maneuver_length_ = declare_parameter("vehicle_id", config_.min_maneuver_length_); } carma_ros2_utils::CallbackReturn RouteFollowingPlugin::on_configure_plugin() { - // Setup service servers - plan_maneuver_srv_= create_service(planning_strategic_plugin_ + "/plan_maneuvers", - std::bind(&RouteFollowingPlugin::plan_maneuvers_callback, this, std_ph::_1, std_ph::_2, std_ph::_3)); - + // Setup publishers upcoming_lane_change_status_pub_ = create_publisher("upcoming_lane_change_status", 1); @@ -122,14 +120,14 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id wml_->setRouteCallback([this]() { RCLCPP_INFO_STREAM(get_logger(),"Recomputing maneuvers due to a route update"); this->latest_maneuver_plan_ = routeCb(wm_->getRoute()->shortestPath()); - }); + }); wml_->setMapCallback([this]() { if (wm_->getRoute()) { // If this map update occured after a route was provided we need to regenerate maneuvers - RCLCPP_INFO_STREAM(get_logger(),"Recomputing maneuvers due to map update"); - this->latest_maneuver_plan_ = routeCb(wm_->getRoute()->shortestPath()); + RCLCPP_INFO_STREAM(get_logger(),"Recomputing maneuvers due to map update"); + this->latest_maneuver_plan_ = routeCb(wm_->getRoute()->shortestPath()); } - }); + }); initializeBumperTransformLookup(); diff --git a/stop_and_wait_plugin/include/stop_and_wait_node.hpp b/stop_and_wait_plugin/include/stop_and_wait_node.hpp index 5125760b5b..b181729d6b 100644 --- a/stop_and_wait_plugin/include/stop_and_wait_node.hpp +++ b/stop_and_wait_plugin/include/stop_and_wait_node.hpp @@ -60,7 +60,6 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin std::string version_id_; std::string plugin_name_; - carma_wm::WorldModelConstPtr wm_; public: @@ -74,6 +73,12 @@ class StopandWaitNode : public carma_guidance_plugins::TacticalPlugin */ carma_ros2_utils::CallbackReturn on_configure_plugin(); + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + //// // Overrides //// diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp index 96d9d6defc..00612ebd7c 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin_node.cpp @@ -21,7 +21,7 @@ namespace stop_and_wait_plugin namespace std_ph = std::placeholders; StopandWaitNode::StopandWaitNode(const rclcpp::NodeOptions &options) - : carma_guidance_plugins::TacticalPlugin(options),version_id_("v1.0"),plugin_name_(get_plugin_name_and_ns()) + : carma_guidance_plugins::TacticalPlugin(options),version_id_("v4.0"),plugin_name_(get_plugin_name_and_ns()) { // Create initial config config_ = StopandWaitConfig(); @@ -31,18 +31,50 @@ namespace stop_and_wait_plugin config_.stop_timestep = declare_parameter("stop_timestep", config_.stop_timestep); config_.trajectory_step_size = declare_parameter("trajectory_step_size", config_.trajectory_step_size); config_.accel_limit_multiplier = declare_parameter("accel_limit_multiplier", config_.accel_limit_multiplier); - config_.accel_limit = declare_parameter("/vehicle_acceleration_limit", config_.accel_limit); + config_.accel_limit = declare_parameter("vehicle_acceleration_limit", config_.accel_limit); config_.crawl_speed = declare_parameter("crawl_speed", config_.crawl_speed); config_.centerline_sampling_spacing = declare_parameter("centerline_sampling_spacing", config_.centerline_sampling_spacing); config_.default_stopping_buffer = declare_parameter("default_stopping_buffer", config_.default_stopping_buffer); } + rcl_interfaces::msg::SetParametersResult StopandWaitNode::parameter_update_callback(const std::vector ¶meters) + { + + auto error = update_params({ + {"minimal_trajectory_duration", config_.minimal_trajectory_duration}, + {"stop_timestep", config_.stop_timestep}, + {"trajectory_step_size", config_.trajectory_step_size}, + {"accel_limit_multiplier", config_.accel_limit_multiplier}, + {"crawl_speed", config_.crawl_speed}, + {"centerline_sampling_spacing", config_.centerline_sampling_spacing}, + {"default_stopping_buffer", config_.default_stopping_buffer} + }, parameters); // vehicle_acceleration_limit not updated as it's global param + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error; + + return result; + } + carma_ros2_utils::CallbackReturn StopandWaitNode::on_configure_plugin() { + + get_parameter("minimal_trajectory_duration", config_.minimal_trajectory_duration); + get_parameter("stop_timestep", config_.stop_timestep); + get_parameter("trajectory_step_size", config_.trajectory_step_size); + get_parameter("accel_limit_multiplier", config_.accel_limit_multiplier); + get_parameter("vehicle_acceleration_limit", config_.accel_limit); + get_parameter("crawl_speed", config_.crawl_speed); + get_parameter("centerline_sampling_spacing", config_.centerline_sampling_spacing); + get_parameter("default_stopping_buffer", config_.default_stopping_buffer); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&StopandWaitNode::parameter_update_callback, this, std_ph::_1)); - plugin_ = std::make_shared(shared_from_this(), wm_, config_,plugin_name_,version_id_); + plugin_ = std::make_shared(shared_from_this(), get_world_model(), config_,plugin_name_,version_id_); - // Return success if everthing initialized successfully + // Return success if everything initialized successfully return CallbackReturn::SUCCESS; } From 0cfaf6f8101eae1733aff20d12e0ce0f988f9987 Mon Sep 17 00:00:00 2001 From: JonSmet <34752540+JonSmet@users.noreply.github.com> Date: Wed, 24 Aug 2022 10:03:35 -0400 Subject: [PATCH 084/165] Port cooperative_lanechange to ROS2 (#1903) Description This PR includes updates to port the cooperative_lanechange plugin from ROS1 to ROS2. Related Issue Issue #1885 How Has This Been Tested? All unit tests pass, and local integration tests have been conducted. --- carma/launch/guidance.launch | 6 - carma/launch/plugins.launch.py | 54 +- cooperative_lanechange/CMakeLists.txt | 125 +--- cooperative_lanechange/README.md | 5 + cooperative_lanechange/config/parameters.yaml | 47 +- .../include/cooperative_lanechange.h | 271 ------- .../cooperative_lanechange_config.hpp | 92 +++ .../cooperative_lanechange_node.hpp | 254 +++++++ .../launch/cooperative_lanechange.launch | 27 - .../launch/cooperative_lanechange_launch.py | 68 ++ cooperative_lanechange/package.xml | 64 +- .../src/cooperative_lanechange.cpp | 588 ---------------- .../src/cooperative_lanechange_node.cpp | 661 ++++++++++++++++++ cooperative_lanechange/src/main.cpp | 25 +- .../test/cooperative_lanechange.test | 21 - .../test/test_cooperative_plugin.cpp | 302 ++++---- .../config/guidance_controller_config.yaml | 1 + 17 files changed, 1409 insertions(+), 1202 deletions(-) create mode 100644 cooperative_lanechange/README.md delete mode 100644 cooperative_lanechange/include/cooperative_lanechange.h create mode 100644 cooperative_lanechange/include/cooperative_lanechange/cooperative_lanechange_config.hpp create mode 100644 cooperative_lanechange/include/cooperative_lanechange/cooperative_lanechange_node.hpp delete mode 100644 cooperative_lanechange/launch/cooperative_lanechange.launch create mode 100644 cooperative_lanechange/launch/cooperative_lanechange_launch.py delete mode 100644 cooperative_lanechange/src/cooperative_lanechange.cpp create mode 100644 cooperative_lanechange/src/cooperative_lanechange_node.cpp delete mode 100644 cooperative_lanechange/test/cooperative_lanechange.test diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index c32ffbc52d..9e0e6d4f4b 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -143,12 +143,6 @@ - - - - - - diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 3cefa1b9b6..01908e462f 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -51,7 +51,10 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + + cooperative_lanechange_param_file = os.path.join( + get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml') platoon_strategic_ihp_param_file = os.path.join( get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') @@ -68,10 +71,10 @@ def generate_launch_description(): package='inlanecruising_plugin', plugin='inlanecruising_plugin::InLaneCruisingPluginNode', name='inlanecruising_plugin', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('inlanecruising_plugin', env_log_levels) } - ], + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('inlanecruising_plugin', env_log_levels) } + ], remappings = [ ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ), ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ), @@ -90,9 +93,9 @@ def generate_launch_description(): plugin='stop_and_wait_plugin::StopandWaitNode', name='stop_and_wait_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('stop_and_wait_plugin', env_log_levels) } - ], + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('stop_and_wait_plugin', env_log_levels) } + ], remappings = [ ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ), ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ), @@ -111,9 +114,9 @@ def generate_launch_description(): plugin='route_following_plugin::RouteFollowingPlugin', name='route_following_plugin', extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('route_following_plugin', env_log_levels) } - ], + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('route_following_plugin', env_log_levels) } + ], remappings = [ ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ), ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ), @@ -129,6 +132,35 @@ def generate_launch_description(): route_following_plugin_file_path, vehicle_config_param_file ] + ), + ComposableNode( + package='cooperative_lanechange', + plugin='cooperative_lanechange::CooperativeLaneChangePlugin', + name='cooperative_lanechange', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cooperative_lanechange', env_log_levels) } + ], + 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" ] ), + ("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" ] ), + ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ("cooperative_lane_change_status", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/cooperative_lane_change_status" ] ), + ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ), + ("outgoing_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_request" ] ), + ("incoming_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_response" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ) + ], + parameters=[ + cooperative_lanechange_param_file, + vehicle_characteristics_param_file, + vehicle_config_param_file + ] ) ] ) diff --git a/cooperative_lanechange/CMakeLists.txt b/cooperative_lanechange/CMakeLists.txt index 0e9ab275b3..e72dc709be 100644 --- a/cooperative_lanechange/CMakeLists.txt +++ b/cooperative_lanechange/CMakeLists.txt @@ -1,5 +1,5 @@ -# -# Copyright (C) 2021 LEIDOS. + +# 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 @@ -12,112 +12,61 @@ # 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.0.2) + +cmake_minimum_required(VERSION 3.5) project(cooperative_lanechange) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -set( CATKIN_DEPS - roscpp - std_msgs - cav_msgs - cav_srvs - carma_utils - trajectory_utils - autoware_msgs - carma_wm - basic_autonomy - tf - tf2_ros - tf2_geometry_msgs - roslib - carma_debug_msgs - lanelet2_extension -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) -## System dependencies are found with CMake's conventions -find_package(Boost) -find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### - - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost EIGEN3 -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -########### -## Build ## -########### +# Name build targets +set(node_exec cooperative_lanechange_node_exec) +set(node_lib cooperative_lanechange_node) -## Specify additional locations of header files -## Your package locations should be listed before other locations +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} ) -file(GLOB_RECURSE headers */*.hpp */*.h) +# Build +ament_auto_add_library(${node_lib} SHARED + src/cooperative_lanechange_node.cpp +) -add_executable( ${PROJECT_NAME} - src/main.cpp) +ament_auto_add_executable(${node_exec} + src/main.cpp +) +# Register component +rclcpp_components_register_nodes(${node_lib} "cooperative_lanechange::CooperativeLaneChangePlugin") -add_library(cooperative_lanechange_plugin_lib -src/cooperative_lanechange.cpp +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -add_dependencies(cooperative_lanechange_plugin_lib ${catkin_EXPORTED_TARGETS}) +# Testing +if(BUILD_TESTING) -target_link_libraries(cooperative_lanechange_plugin_lib ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} cooperative_lanechange_plugin_lib) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + ament_add_gtest(test_cooperative_lanechange test/test_cooperative_plugin.cpp) -############# -## Install ## -############# + ament_target_dependencies(test_cooperative_lanechange ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + target_link_libraries(test_cooperative_lanechange ${node_lib}) -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Install C++ -install(TARGETS ${PROJECT_NAME} cooperative_lanechange_plugin_lib - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +endif() -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch ) - - -############# -## Testing ## -############# -# catkin_add_gmock(${PROJECT_NAME}-test -# test/test_cooperative_lanechange.cpp -# WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) -# target_link_libraries(${PROJECT_NAME}-test cooperative_lanechange_plugin_lib ${catkin_LIBRARIES}) - -catkin_add_gmock(${PROJECT_NAME}-test test/test_cooperative_plugin.cpp) -target_link_libraries(${PROJECT_NAME}-test cooperative_lanechange_plugin_lib ${catkin_LIBRARIES}) - diff --git a/cooperative_lanechange/README.md b/cooperative_lanechange/README.md new file mode 100644 index 0000000000..5a6908c627 --- /dev/null +++ b/cooperative_lanechange/README.md @@ -0,0 +1,5 @@ +# cooperative_lanechange + +The cooperative_lanechange tactical plugin is responsible for converting high-level lane change maneuvers into detailed lane change trajectories. This plugin generates lane change trajectories for two types of scenarios: (1) "Regular Planning" in which no vehicles are near the host vehicle in the target lane, and (2) "Negotiation" in which communication with a neighboring vehicle is required to open a gap for a safe lane change. + +Link to detailed design document on Confluence: [Click Here](https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/1318715418/Detailed+Design+-+Cooperative+Lanechange) \ No newline at end of file diff --git a/cooperative_lanechange/config/parameters.yaml b/cooperative_lanechange/config/parameters.yaml index 480824e8f2..041abbc201 100644 --- a/cooperative_lanechange/config/parameters.yaml +++ b/cooperative_lanechange/config/parameters.yaml @@ -1,4 +1,4 @@ -# Copyright (C) 2019-2020 LEIDOS. +# 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 @@ -51,7 +51,7 @@ lateral_accel_limit : 1.5 # Int : Size of the window used in the moving average filter to smooth both the computed curvature and output speeds speed_moving_average_window_size : 5 -#Int: // Size of the window used in the moving average filter to smooth the curvature profile +# Int : Size of the window used in the moving average filter to smooth the curvature profile curvature_moving_average_window_size : 9 # Int : Number of points to look ahead when calculating the curvature of the lanelet centerline @@ -60,41 +60,48 @@ curvature_calc_lookahead_count : 1 # Int: Amount to downsample input lanelet centerline data. Value corresponds to saving each nth point. downsample_ratio : 1 -#double : Acceptable range from the ending downtrack, when planning success can be published, if vehicle has changed lanes -#Units: meters -destination_range : 0 +# Double : Acceptable range from the ending downtrack, when planning success can be published, if vehicle has changed lanes +# Units: meters +destination_range : 0.0 -#Double : The time after which a lane change request can be considered to time out -#Units : seconds +# Double : The time after which a lane change request can be considered to time out +# Units : seconds lanechange_time_out: 6.0 # Double: The minimum timestep used for planning trajectory -#Units: seconds +# Units: seconds min_timestep : 0.1 -#Double: The range from the start from which the plugin can start working -#Units: meters +# Double: The range from the start from which the plugin can start working +# Units: meters starting_downtrack_range : 5.0 -#Double: Fraction of the maneuver that has been completed. Starting fraction specifies a higher urgency of in the request to lane change +# Double: Fraction of the maneuver that has been completed. Starting fraction specifies a higher urgency of in the request to lane change starting_fraction : 0.2 -#Double: Fraction of the maneuver that has been completed. Mid fraction specifies a lower urgency in the request to lane change +# Double: Fraction of the maneuver that has been completed. Mid fraction specifies a lower urgency in the request to lane change mid_fraction : 0.5 -#Double: The minimum gap required between 2 vehicles to start lane changing -#Units: meters +# Double: The minimum gap required between 2 vehicles to start lane changing +# Units: meters min_desired_gap: 5.0 -##Unused parameters added for using basic autonomy library -#Int: Amount to downsample input lanelet centerline data on turns. Value corresponds to saving each nth point. +# Double: Maximum time gap with vehicle in adjacent lane to require negotiation for lane change +# Units: seconds +desired_time_gap: 3.0 + +## Unused parameters added for using basic autonomy library +# Int: Amount to downsample input lanelet centerline data on turns. Value corresponds to saving each nth point. turn_downsample_ratio: 0 -#Double: Curve re-sampling step size in m +# Double: Curve re-sampling step size +# Units: meters curve_resample_step_size: 1.0 -#Double: Number of meters behind the first maneuver that need to be included in points for curvature calculation +# Double: Number of meters behind the first maneuver that need to be included in points for curvature calculation +# Units: meters back_distance: 0.0 -#Double: -buffer_ending_downtrack: 5.0 +# Double: Additional distance beyond ending downtrack to ensure sufficient points +# Units: meters +buffer_ending_downtrack: 5.0 \ No newline at end of file diff --git a/cooperative_lanechange/include/cooperative_lanechange.h b/cooperative_lanechange/include/cooperative_lanechange.h deleted file mode 100644 index f038a6b545..0000000000 --- a/cooperative_lanechange/include/cooperative_lanechange.h +++ /dev/null @@ -1,271 +0,0 @@ -#pragma once -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - - -namespace cooperative_lanechange -{ - // Helpful using declarations - using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; - - /** - * \brief Convenience struct for storing the original start_dist and starting_lane_id associated - * with a received lane change maneuver. - */ - struct LaneChangeManeuverOriginalValues - { - std::string maneuver_id; // maneuver_id that this object corresponds to - std::string original_starting_lane_id; // Original starting_lane_id associated with this lane change maneuver - double original_start_dist; // Original start_dist associated with this lane change maneuver - double original_longitudinal_vel_ms; // The vehicle velocity (in m/s) when the vehicle first began this lane change - bool has_started = false; // Flag to indicate whether the vehicle's downtrack is beyond the original_start_dist of this lane change maneuver - }; - - class CooperativeLaneChangePlugin - { - public: - /** - * \brief General entry point to begin the operation of this class - */ - void run(); - - /** - * \brief Service callback for trajectory planning - * - * \param req The service request - * \param resp The service response - * - * \return True if success. False otherwise - */ - bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest &req, cav_srvs::PlanTrajectoryResponse &resp); - /** - * \brief Creates a vector of Trajectory Points from maneuver information in trajectory request - * - * \param req The service request - * - * \return vector of unobstructed lane change trajectory points - */ - std::vector plan_lanechange(cav_srvs::PlanTrajectoryRequest &req); - /** - * \brief Calculates distance between subject vehicle and vehicle 2 - * - * \param veh2_lanelet_id Current lanelet id of vehicle 2 - * \param veh2_downtrack Downtrack of vehicle 2 in its current lanelet - * \param ego_state vehicle state of the ego vehicle - * - * \return the distance between subject vehicle and vehicle 2 - */ - double find_current_gap(long veh2_lanelet_id, double veh2_downtrack, cav_msgs::VehicleState& ego_state) const ; - - /** - * \brief Callback to subscribed mobility response topic - * \param msg Latest mobility response message - */ - void mobilityresponse_cb(const cav_msgs::MobilityResponse& msg); - - /** - * \brief Creates a mobility request message from planned trajectory and requested maneuver info - * \param trajectory_plan A vector of lane change trajectory points - * \param The mobility request message created from trajectory points, for publishing - */ - cav_msgs::MobilityRequest create_mobility_request(std::vector& trajectory_plan, cav_msgs::Maneuver& maneuver); - - /** - * \brief Converts Trajectory Plan to (Mobility) Trajectory - * \param traj_points vector of Trajectory Plan points to be converted to Trajectory type message - * \return The Trajectory type message in world frame - */ - - cav_msgs::Trajectory trajectory_plan_to_trajectory(const std::vector& traj_points) const; - /** - * \brief Converts Trajectory Point to ECEF frame using map projection - * \param traj_points A Trajectory Plan point to be converted to Trajectory type message - * \throw std::invalid_argument If the map_projector_ member variable has not been set - * \return The trajectory point message transformed to ecef frame - */ - cav_msgs::LocationECEF trajectory_point_to_ecef(const cav_msgs::TrajectoryPlanPoint& traj_point) const; - - void add_maneuver_to_response(cav_srvs::PlanTrajectoryRequest &req, cav_srvs::PlanTrajectoryResponse &resp, std::vector& planned_trajectory_points); - - /** - * \brief Given the curvature fit, computes the curvature at the given step along the curve - * - * \param step_along_the_curve Value in double from 0.0 (curvature start) to 1.0 (curvature end) representing where to calculate the curvature - * - * \param fit_curve curvature fit - * - * \return Curvature (k = 1/r, 1/meter) - */ - // initialize this node - void initialize(); - - /** - * \brief Callback for map projection string to define lat/lon -> map conversion - * \brief msg The proj string defining the projection. - */ - void georeference_callback(const std_msgs::StringConstPtr& msg); - - //Internal Variables used in unit testsis_lanechange_accepted_ - // Current vehicle forward speed - double current_speed_; - - // Current vehicle pose in map - geometry_msgs::PoseStamped pose_msg_; - - // wm listener pointer and pointer to the actual wm object - std::shared_ptr wml_; - carma_wm::WorldModelConstPtr wm_; - - //boolean which is updated if lane change request is accepted - bool is_lanechange_accepted_ = false; - - ros::Publisher outgoing_mobility_request_; - ros::Publisher lanechange_status_pub_; - - cav_msgs::VehicleState ending_state_before_buffer_; - - private: - - // node handles - std::shared_ptr nh_, pnh_; - - ros::Publisher cooperative_lanechange_plugin_discovery_pub_; - - // ros service servers - ros::ServiceServer trajectory_srv_; - - // ROS publishers and subscribers - cav_msgs::Plugin plugin_discovery_msg_; - ros::Subscriber pose_sub_; - ros::Subscriber twist_sub_; - - ros::Subscriber incoming_mobility_response_; - ros::Subscriber bsm_sub_; - ros::Subscriber georeference_sub_; - ros::Timer discovery_pub_timer_; - - std::shared_ptr map_projector_; - - // trajectory frequency - double traj_freq = 10; - std::string DEFAULT_STRING_= ""; - //Time at which the request is first sent - ros::Time request_sent_time; - //boolean that records whether request has already been sent - bool request_sent = false; - //fraction of the maneuver completed - double maneuver_fraction_completed_ = 0; - // flag to check if CLC plugin is called - bool clc_called_ = false; - // Mobility request id - std::string clc_request_id_ = "default_request_id"; - // ROS params - //Vehicle params - std::string sender_id_ = DEFAULT_STRING_; - cav_msgs::BSMCoreData bsm_core_; - - // Maps maneuver IDs to their corresponding LaneChangeManeuverOriginalValues object - std::unordered_map original_lc_maneuver_values_; - - //Plugin specific params - double desired_time_gap_ = 3.0; - double trajectory_time_length_ = 6; - std::string control_plugin_name_ = "pure_pursuit"; - double minimum_speed_ = 2.0; - double max_accel_ = 1.5; - double minimum_lookahead_distance_ = 5.0; - double maximum_lookahead_distance_ = 25.0; - double minimum_lookahead_speed_ = 2.8; - double maximum_lookahead_speed_ =13.9; - double lateral_accel_limit_ = 1.5; - double speed_moving_average_window_size_ = 5; - double curvature_moving_average_window_size_ = 5; - double curvature_calc_lookahead_count_ = 1; - int downsample_ratio_ =8; - double destination_range_ = 5; - double lanechange_time_out_ = 6.0; - int num_points = traj_freq * trajectory_time_length_; - double min_timestep_ = 0.1; - double starting_downtrack_range_ = 5.0; //This parameter dictates how long before the start_dist, is it ok for the plugin to work if it's called early - double starting_fraction_ = 0.2; - double mid_fraction_ = 0.5; - double min_desired_gap_ =5.0; - - int turn_downsample_ratio_ = 0.0; - double curve_resample_step_size_ = 0.0; - double back_distance_ = 0.0; - double buffer_ending_downtrack_ = 0.0; - - - - // generated trajectory plan - cav_msgs::TrajectoryPlan trajectory_msg; - - /** - * \brief Callback for the pose subscriber, which will store latest pose locally - * \param msg Latest pose message - */ - void pose_cb(const geometry_msgs::PoseStampedConstPtr& msg); - - /** - * \brief Callback for the twist subscriber, which will store latest twist locally - * \param msg Latest twist message - */ - void twist_cd(const geometry_msgs::TwistStampedConstPtr& msg); - - /** - * \brief Callback to reads bsm message from topic - * \param msg The bsm message obtained from subscribed topic - * message info is stored in class variable - */ - void bsm_cb(const cav_msgs::BSMConstPtr& msg); - - std::string bsmIDtoString(cav_msgs::BSMCoreData bsm_core){ - std::string res = ""; - for (size_t i=0; i +#include + +namespace cooperative_lanechange +{ + + /** + * \brief Stuct containing the algorithm configuration values for cooperative_lanechange + */ + struct Config + { + double trajectory_time_length = 6.0; + std::string control_plugin_name = "pure_pursuit"; + double minimum_speed = 2.2352; + double max_accel = 1.5; + double minimum_lookahead_distance = 5.0; + double maximum_lookahead_distance = 25.0; + double minimum_lookahead_speed = 2.8; + double maximum_lookahead_speed = 13.9; + double lateral_accel_limit = 1.5; + int speed_moving_average_window_size = 5; + int curvature_moving_average_window_size = 9; + int curvature_calc_lookahead_count = 1; + int downsample_ratio = 1; + double destination_range = 0.0; + double lanechange_time_out = 6.0; + double min_timestep = 0.1; + double starting_downtrack_range = 5.0; + double starting_fraction = 0.2; + double mid_fraction = 0.5; + double min_desired_gap = 5.0; + double desired_time_gap = 3.0; + int turn_downsample_ratio = 0; + double curve_resample_step_size = 1.0; + double back_distance = 0.0; + double buffer_ending_downtrack = 5.0; + std::string vehicle_id = "DEFAULT_VEHICLE_ID"; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const Config &c) + { + output << "cooperative_lanechange::Config { " << std::endl + << "trajectory_time_length: " << c.trajectory_time_length << std::endl + << "control_plugin_name: " << c.control_plugin_name << std::endl + << "minimum_speed: " << c.minimum_speed << std::endl + << "max_accel: " << c.max_accel << std::endl + << "minimum_lookahead_distance: " << c.minimum_lookahead_distance << std::endl + << "maximum_lookahead_distance: " << c.maximum_lookahead_distance << std::endl + << "minimum_lookahead_speed: " << c.minimum_lookahead_speed << std::endl + << "maximum_lookahead_speed: " << c.maximum_lookahead_speed << std::endl + << "lateral_accel_limit: " << c.lateral_accel_limit << std::endl + << "speed_moving_average_window_size: " << c.speed_moving_average_window_size << std::endl + << "curvature_moving_average_window_size: " << c.curvature_moving_average_window_size << std::endl + << "curvature_calc_lookahead_count: " << c.curvature_calc_lookahead_count << std::endl + << "downsample_ratio: " << c.downsample_ratio << std::endl + << "destination_range: " << c.destination_range << std::endl + << "lanechange_time_out: " << c.lanechange_time_out << std::endl + << "min_timestep: " << c.min_timestep << std::endl + << "starting_downtrack_range: " << c.starting_downtrack_range << std::endl + << "starting_fraction: " << c.starting_fraction << std::endl + << "mid_fraction: " << c.mid_fraction << std::endl + << "min_desired_gap: " << c.min_desired_gap << std::endl + << "desired_time_gap: " << c.desired_time_gap << std::endl + << "turn_downsample_ratio: " << c.turn_downsample_ratio << std::endl + << "curve_resample_step_size: " << c.curve_resample_step_size << std::endl + << "back_distance: " << c.back_distance << std::endl + << "buffer_ending_downtrack: " << c.buffer_ending_downtrack << std::endl + << "vehicle_id: " << c.vehicle_id << std::endl + << "}" << std::endl; + return output; + } + }; + +} // cooperative_lanechange \ No newline at end of file diff --git a/cooperative_lanechange/include/cooperative_lanechange/cooperative_lanechange_node.hpp b/cooperative_lanechange/include/cooperative_lanechange/cooperative_lanechange_node.hpp new file mode 100644 index 0000000000..80fe343ae6 --- /dev/null +++ b/cooperative_lanechange/include/cooperative_lanechange/cooperative_lanechange_node.hpp @@ -0,0 +1,254 @@ +/* + * 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. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "cooperative_lanechange/cooperative_lanechange_config.hpp" + +namespace cooperative_lanechange +{ + using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; + + /** + * \brief Convenience struct for storing the original start_dist and starting_lane_id associated + * with a received lane change maneuver. + */ + struct LaneChangeManeuverOriginalValues + { + std::string maneuver_id; // maneuver_id that this object corresponds to + std::string original_starting_lane_id; // Original starting_lane_id associated with this lane change maneuver + double original_start_dist; // Original start_dist associated with this lane change maneuver + double original_longitudinal_vel_ms; // The vehicle velocity (in m/s) when the vehicle first began this lane change + bool has_started = false; // Flag to indicate whether the vehicle's downtrack is beyond the original_start_dist of this lane change maneuver + }; + + /** + * \class CooperativeLaneChangePlugin + * \brief The class responsible for generating cooperative lanechange trajectories from received lane change maneuvers + */ + class CooperativeLaneChangePlugin : public carma_guidance_plugins::TacticalPlugin + { + + private: + // Subscribers + carma_ros2_utils::SubPtr pose_sub_; + carma_ros2_utils::SubPtr twist_sub_; + carma_ros2_utils::SubPtr incoming_mobility_response_sub_; + carma_ros2_utils::SubPtr georeference_sub_; + carma_ros2_utils::SubPtr bsm_sub_; + + // Publishers + carma_ros2_utils::PubPtr outgoing_mobility_request_pub_; + carma_ros2_utils::PubPtr lanechange_status_pub_; + + // CooperativeLaneChangePlugin configuration + Config config_; + + // World Model object + carma_wm::WorldModelConstPtr wm_; + + // Map projection string, which defines the lat/lon -> map conversion + std::shared_ptr map_projector_; + + // Trajectory frequency + double traj_freq_ = 10; + + // Default recipient ID to be used for cooperative lane change Mobility Requests + std::string DEFAULT_STRING_= ""; + + // Time at which the cooperative lane change Mobility Request is first sent + rclcpp::Time request_sent_time_; + + // Boolean that records whether request has already been sent + bool request_sent_ = false; + + // Fraction of the received maneuver that has already been completed by the host vehicle + double maneuver_fraction_completed_ = 0; + + // Boolean to check if CLC plugin's plan trajectory service has been called + bool clc_called_ = false; + + // The plan ID associated with the cooperative lane change Mobility Request; initialized with a default value + std::string clc_request_id_ = "default_request_id"; + + // The latest BSM Core Data broadcasted by the host vehicle + carma_v2x_msgs::msg::BSMCoreData bsm_core_; + + // Maps maneuver IDs to their corresponding LaneChangeManeuverOriginalValues object + std::unordered_map original_lc_maneuver_values_; + + /** + * \brief Callback for the pose subscriber, which will store latest pose locally + * \param msg Latest pose message + */ + void pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg); + + /** + * \brief Callback for the twist subscriber, which will store latest twist locally + * \param msg Latest twist message + */ + void twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg); + + /** + * \brief Callback for the BSM subscriber, which will store the latest BSM Core Data broadcasted by the host vehicle + * \param msg Latest BSM message + */ + void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg); + + /** + * \brief Method for extracting the BSM ID from a BSM Core Data object and converting it to a string + * \param bsm_core The BSM Core Data object from which the BSM ID is converted to a string + * \return The BSM ID (as a string) from the BSM Core Data object + */ + std::string bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core); + + public: + // Internal Variables used in unit testsis_lanechange_accepted_ + // Current vehicle forward speed + double current_speed_; + + // Current vehicle pose in map + geometry_msgs::msg::PoseStamped pose_msg_; + + // Boolean flag which is updated if lane change request is accepted + bool is_lanechange_accepted_ = false; + + carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; + + /** + * \brief CooperativeLaneChangePlugin constructor + */ + explicit CooperativeLaneChangePlugin(const rclcpp::NodeOptions &); + + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief Creates a vector of Trajectory Points from maneuver information in trajectory request + * + * \param req The service request + * + * \return vector of unobstructed lane change trajectory points + */ + std::vector plan_lanechange(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req); + + /** + * \brief Calculates distance between subject vehicle and vehicle 2 + * + * \param veh2_lanelet_id Current lanelet id of vehicle 2 + * \param veh2_downtrack Downtrack of vehicle 2 in its current lanelet + * \param ego_state vehicle state of the ego vehicle + * + * \return the distance between subject vehicle and vehicle 2 + */ + double find_current_gap(long veh2_lanelet_id, double veh2_downtrack, carma_planning_msgs::msg::VehicleState& ego_state) const; + + /** + * \brief Callback to subscribed mobility response topic + * \param msg Latest mobility response message + */ + void mobilityresponse_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg); + + /** + * \brief Creates a mobility request message from planned trajectory and requested maneuver info + * \param trajectory_plan A vector of lane change trajectory points + * \param The mobility request message created from trajectory points, for publishing + */ + carma_v2x_msgs::msg::MobilityRequest create_mobility_request(std::vector& trajectory_plan, carma_planning_msgs::msg::Maneuver& maneuver); + + /** + * \brief Converts Trajectory Plan to (Mobility) Trajectory + * \param traj_points vector of Trajectory Plan points to be converted to Trajectory type message + * \return The Trajectory type message in world frame + */ + carma_v2x_msgs::msg::Trajectory trajectory_plan_to_trajectory(const std::vector& traj_points) const; + + /** + * \brief Converts Trajectory Point to ECEF frame using map projection + * \param traj_points A Trajectory Plan point to be converted to Trajectory type message + * \throw std::invalid_argument If the map_projector_ member variable has not been set + * \return The trajectory point message transformed to ecef frame + */ + carma_v2x_msgs::msg::LocationECEF trajectory_point_to_ecef(const carma_planning_msgs::msg::TrajectoryPlanPoint& traj_point) const; + + /** + * \brief Adds the generated trajectory plan to the service response + * \param req The plan trajectory service request + * \param resp The plan trajectory service response + * \param planned_trajectory_points The generated trajectory plan points, which are added to the service response + */ + void add_trajectory_to_response(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, + const std::vector& planned_trajectory_points); + + /** + * \brief Callback for map projection string to define lat/lon -> map conversion + * \brief msg The proj string defining the projection. + */ + void georeference_cb(const std_msgs::msg::String::UniquePtr msg); + + //// + // Overrides + //// + void plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; + + bool get_availability() override; + + std::string get_version_id() override; + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + + // Unit Tests + FRIEND_TEST(CooperativeLaneChangePlugin, TestLaneChangefunctions); + FRIEND_TEST(CooperativeLaneChangePlugin, Testcurrentgapcb); + FRIEND_TEST(CooperativeLaneChangePlugin,TestNoPath_roadwayobject); + }; + +} // cooperative_lanechange diff --git a/cooperative_lanechange/launch/cooperative_lanechange.launch b/cooperative_lanechange/launch/cooperative_lanechange.launch deleted file mode 100644 index 3740b25ee8..0000000000 --- a/cooperative_lanechange/launch/cooperative_lanechange.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/cooperative_lanechange/launch/cooperative_lanechange_launch.py b/cooperative_lanechange/launch/cooperative_lanechange_launch.py new file mode 100644 index 0000000000..ea38969406 --- /dev/null +++ b/cooperative_lanechange/launch/cooperative_lanechange_launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA cooperative_lanechange_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='cooperative_lanechange_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='cooperative_lanechange', + plugin='cooperative_lanechange::CooperativeLaneChangePlugin', + name='cooperative_lanechange', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/cooperative_lanechange/package.xml b/cooperative_lanechange/package.xml index 5ac78699db..6c3e1fe876 100644 --- a/cooperative_lanechange/package.xml +++ b/cooperative_lanechange/package.xml @@ -1,25 +1,51 @@ + + + cooperative_lanechange - 3.3.0 - The node is to plan a cooperative lane change trajectory - carma - Apache 2.0 License - catkin - roscpp + 4.0.0 + The cooperative_lanechange package + + carma + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + carma_guidance_plugins + geometry_msgs + lanelet2_core + carma_v2x_msgs + carma_planning_msgs + basic_autonomy_ros2 std_msgs - cav_srvs - cav_msgs - carma_utils - autoware_msgs - carma_wm - tf - tf2_ros - tf2_geometry_msgs - trajectory_utils - roslib - basic_autonomy - carma_debug_msgs lanelet2_extension - carma_cmake_common + carma_perception_msgs + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + diff --git a/cooperative_lanechange/src/cooperative_lanechange.cpp b/cooperative_lanechange/src/cooperative_lanechange.cpp deleted file mode 100644 index 9c081f5b03..0000000000 --- a/cooperative_lanechange/src/cooperative_lanechange.cpp +++ /dev/null @@ -1,588 +0,0 @@ -/* - * Copyright (C) 2019-2020 LEIDOS. - * - * Licensed under the Apache License, Version 2.0 (the "License"); you may not - * use this file except in compliance with the License. You may obtain a copy of - * the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations under - * the License. - */ -#include -#include -#include -#include -#include "cooperative_lanechange.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace cooperative_lanechange -{ - void CooperativeLaneChangePlugin::initialize() - { - //@SONAR_STOP@ - nh_.reset(new ros::CARMANodeHandle()); - pnh_.reset(new ros::CARMANodeHandle("~")); - - trajectory_srv_ = nh_->advertiseService("cooperative_lanechange/plan_trajectory", &CooperativeLaneChangePlugin::plan_trajectory_cb, this); - - cooperative_lanechange_plugin_discovery_pub_ = nh_->advertise("plugin_discovery", 1); - plugin_discovery_msg_.name = "cooperative_lanechange"; - plugin_discovery_msg_.version_id = "v1.0"; - plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = true; - plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; - plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; - - pose_sub_ = nh_->subscribe("current_pose", 1, &CooperativeLaneChangePlugin::pose_cb, this); - twist_sub_ = nh_->subscribe("current_velocity", 1, &CooperativeLaneChangePlugin::twist_cd, this); - incoming_mobility_response_ = nh_->subscribe("incoming_mobility_response", 1 , &CooperativeLaneChangePlugin::mobilityresponse_cb, this); - - georeference_sub_ = nh_->subscribe("georeference", 1, &CooperativeLaneChangePlugin::georeference_callback, this); - - bsm_sub_ = nh_->subscribe("bsm_outbound", 1, &CooperativeLaneChangePlugin::bsm_cb, this); - outgoing_mobility_request_ = nh_->advertise("outgoing_mobility_request", 5); // rate from yield plugin - lanechange_status_pub_ = nh_->advertise("cooperative_lane_change_status",10); - //Vehicle params - pnh_->getParam("vehicle_id",sender_id_); - - //Plugin params - pnh_->param("trajectory_time_length", trajectory_time_length_, 6.0); - pnh_->param("control_plugin_name", control_plugin_name_, "NULL"); - pnh_->param("minimum_speed", minimum_speed_, 2.2352); - pnh_->param("max_accel", max_accel_, 1.5); - pnh_->param("minimum_lookahead_distance", minimum_lookahead_distance_, 5.0); - pnh_->param("maximum_lookahead_distance", maximum_lookahead_distance_, 25.0); - pnh_->param("minimum_lookahead_speed", minimum_lookahead_speed_, 2.8); - pnh_->param("maximum_lookahead_speed", maximum_lookahead_speed_, 13.9); - pnh_->param("lateral_accel_limit", lateral_accel_limit_, 1.5); - pnh_->param("speed_moving_average_window_size", speed_moving_average_window_size_, 5); - pnh_->param("curvature_moving_average_window_size", curvature_moving_average_window_size_, 5); - pnh_->param("curvature_calc_lookahead_count", curvature_calc_lookahead_count_, 1); - pnh_->param("downsample_ratio", downsample_ratio_, 8); - pnh_->param("destination_range",destination_range_, 5); - pnh_->param("lanechange_time_out",lanechange_time_out_, 6.0); - pnh_->param("min_timestep",min_timestep_); - pnh_->param("starting_downtrack_range", starting_downtrack_range_, 5.0); - pnh_->param("starting_fraction", starting_fraction_, 0.2); - pnh_->param("mid_fraction",mid_fraction_, 0.5); - pnh_->param("min_desired_gap",min_desired_gap_, 5.0); - pnh_->param("turn_downsample_ratio", turn_downsample_ratio_, 0); - pnh_->param("curve_resample_step_size", curve_resample_step_size_, 0); - pnh_->param("back_distance", back_distance_, 0.0); - pnh_->param("buffer_ending_downtrack", buffer_ending_downtrack_, 0); - - wml_.reset(new carma_wm::WMListener()); - // set world model point form wm listener - wm_ = wml_->getWorldModel(); - - discovery_pub_timer_ = pnh_->createTimer( - ros::Duration(ros::Rate(10.0)), - [this](const auto&) { cooperative_lanechange_plugin_discovery_pub_.publish(plugin_discovery_msg_); }); - - //@SONAR_START@ - } - - void CooperativeLaneChangePlugin::mobilityresponse_cb(const cav_msgs::MobilityResponse &msg){ - //@SONAR_STOP@ - if (clc_called_ && clc_request_id_ == msg.m_header.plan_id) - { - cav_msgs::LaneChangeStatus lc_status_msg; - if(msg.is_accepted) - { - is_lanechange_accepted_ = true; - lc_status_msg.status = cav_msgs::LaneChangeStatus::ACCEPTANCE_RECEIVED; - lc_status_msg.description = "Received lane merge acceptance"; - } - else - { - is_lanechange_accepted_ = false; - lc_status_msg.status = cav_msgs::LaneChangeStatus::REJECTION_RECEIVED; - lc_status_msg.description = "Received lane merge rejection"; - } - lanechange_status_pub_.publish(lc_status_msg); - //@SONAR_START@ - } - else - { - ROS_DEBUG_STREAM("received mobility response is not related to CLC"); - } - - } - - - double CooperativeLaneChangePlugin::find_current_gap(long veh2_lanelet_id, double veh2_downtrack, cav_msgs::VehicleState& ego_state) const { - - //find downtrack distance between ego and lag vehicle - ROS_DEBUG_STREAM("entered find_current_gap"); - double current_gap = 0.0; - lanelet::BasicPoint2d ego_pos(ego_state.x_pos_global, ego_state.y_pos_global); - //double ego_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack; - - lanelet::LaneletMapConstPtr const_map(wm_->getMap()); - lanelet::ConstLanelet veh2_lanelet = const_map->laneletLayer.get(veh2_lanelet_id); - ROS_DEBUG_STREAM("veh2_lanelet id " << veh2_lanelet.id()); - - auto current_lanelets = lanelet::geometry::findNearest(const_map->laneletLayer, ego_pos, 10); - if(current_lanelets.size() == 0) - { - ROS_WARN_STREAM("Cannot find any lanelet in map!"); - return true; - } - lanelet::ConstLanelet current_lanelet = current_lanelets[0].second; - ROS_DEBUG_STREAM("current llt id " << current_lanelet.id()); - - //Create temporary route between the two vehicles - lanelet::ConstLanelet start_lanelet = veh2_lanelet; - lanelet::ConstLanelet end_lanelet = current_lanelet; - - auto map_graph = wm_->getMapRoutingGraph(); - ROS_DEBUG_STREAM("Graph created"); - - auto temp_route = map_graph->getRoute(start_lanelet, end_lanelet); - ROS_DEBUG_STREAM("Route created"); - - //Throw exception if there is no shortest path from veh2 to subject vehicle - lanelet::routing::LaneletPath shortest_path2; - if(temp_route) - { - shortest_path2 = temp_route.get().shortestPath(); - } - else{ - ROS_ERROR_STREAM("No path exists from roadway object to subject"); - throw std::invalid_argument("No path exists from roadway object to subject"); - } - - ROS_DEBUG_STREAM("Shorted path created size: " << shortest_path2.size()); - for (auto llt : shortest_path2) - { - ROS_DEBUG_STREAM("llt id route: " << llt.id()); - } - - - //To find downtrack- creating temporary route from veh2 to veh1(ego vehicle) - - double veh1_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack; - ROS_DEBUG_STREAM("ego_current_downtrack:" << veh1_current_downtrack); - - - current_gap = veh1_current_downtrack - veh2_downtrack; - ROS_DEBUG_STREAM("Finding current gap"); - ROS_DEBUG_STREAM("Veh1 current downtrack:"<twist.linear.x; - } - - void CooperativeLaneChangePlugin::bsm_cb(const cav_msgs::BSMConstPtr& msg) - { - bsm_core_ = msg->core_data; - - } - - void CooperativeLaneChangePlugin::run() - { - initialize(); - ros::CARMANodeHandle::spin(); - - } - - - - bool CooperativeLaneChangePlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest &req, cav_srvs::PlanTrajectoryResponse &resp){ - //@SONAR_STOP@ - // Only plan the trajectory for the requested LANE_CHANGE maneuver - if (!clc_called_) - { - clc_called_ = true; - } - std::vector maneuver_plan; - if(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan].type != cav_msgs::Maneuver::LANE_CHANGE) - { - throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type"); - } - maneuver_plan.push_back(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan]); - - //Current only checking for first lane change maneuver message - long target_lanelet_id = stol(maneuver_plan[0].lane_change_maneuver.ending_lane_id); - double target_downtrack = maneuver_plan[0].lane_change_maneuver.end_dist; - //get subject vehicle info - lanelet::BasicPoint2d veh_pos(req.vehicle_state.x_pos_global, req.vehicle_state.y_pos_global); - double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack; - - ROS_DEBUG_STREAM("target_lanelet_id: " << target_lanelet_id); - ROS_DEBUG_STREAM("target_downtrack: " << target_downtrack); - ROS_DEBUG_STREAM("current_downtrack: " << current_downtrack); - ROS_DEBUG_STREAM("Starting CLC downtrack: " << maneuver_plan[0].lane_change_maneuver.start_dist); - - if(current_downtrack < maneuver_plan[0].lane_change_maneuver.start_dist - starting_downtrack_range_){ - ROS_DEBUG_STREAM("Lane change trajectory will not be planned. current_downtrack is more than " << starting_downtrack_range_ << " meters before starting CLC downtrack"); - return true; - } - auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, veh_pos, 10); - long current_lanelet_id = current_lanelets[0].second.id(); - if(current_lanelet_id == target_lanelet_id && current_downtrack >= target_downtrack - destination_range_){ - cav_msgs::LaneChangeStatus lc_status_msg; - lc_status_msg.status = cav_msgs::LaneChangeStatus::PLANNING_SUCCESS; - //No description as per UI documentation - lanechange_status_pub_.publish(lc_status_msg); - } - - long veh2_lanelet_id = 0; - double veh2_downtrack = 0.0, veh2_speed = 0.0; - bool foundRoadwayObject = false; - bool negotiate = true; - std::vector rwol = wm_->getRoadwayObjects(); - //Assuming only one connected vehicle in list - for(int i=0;i desired_gap){ - // negotiate = false; //No need for negotiation - // } - - } - else{ - ROS_DEBUG_STREAM("No roadway object"); - //ROS_WARN_STREAM("Did not find a connected and automated vehicle roadway object"); - negotiate = false; - } - - //plan lanechange without filling in response - ROS_DEBUG_STREAM("Planning lane change trajectory"); - - std::string maneuver_id = maneuver_plan[0].lane_change_maneuver.parameters.maneuver_id; - if (original_lc_maneuver_values_.find(maneuver_id) == original_lc_maneuver_values_.end()) { - // If this lane change maneuver ID is being received for this first time, store its original start_dist and starting_lane_id locally - ROS_DEBUG_STREAM("Received maneuver id " << maneuver_id << " for the first time"); - ROS_DEBUG_STREAM("Original start dist is " << maneuver_plan[0].lane_change_maneuver.start_dist); - ROS_DEBUG_STREAM("Original starting_lane_id is " << maneuver_plan[0].lane_change_maneuver.starting_lane_id); - - // Create LaneChangeManeuverOriginalValues object for this lane change maneuver and add it to original_lc_maneuver_values_ - LaneChangeManeuverOriginalValues original_lc_values; - original_lc_values.maneuver_id = maneuver_id; - original_lc_values.original_starting_lane_id = maneuver_plan[0].lane_change_maneuver.starting_lane_id; - original_lc_values.original_start_dist = maneuver_plan[0].lane_change_maneuver.start_dist; - - original_lc_maneuver_values_[maneuver_id] = original_lc_values; - } - else { - // If the vehicle has just started this lane change, store its initial velocity locally; this velocity will be maintained throughout the lane change - if (current_downtrack >= (original_lc_maneuver_values_[maneuver_id]).original_start_dist && !(original_lc_maneuver_values_[maneuver_id]).has_started) { - original_lc_maneuver_values_[maneuver_id].has_started = true; - original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms = std::max(req.vehicle_state.longitudinal_vel, minimum_speed_); - - ROS_DEBUG_STREAM("Lane change maneuver " << maneuver_id << " has started, maintaining speed (in m/s): " << - original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms << " throughout lane change"); - } - } - - std::vector planned_trajectory_points = plan_lanechange(req); - - if(negotiate){ - ROS_DEBUG_STREAM("Negotiating"); - //send mobility request - //Planning for first lane change maneuver - cav_msgs::MobilityRequest request = create_mobility_request(planned_trajectory_points, maneuver_plan[0]); - outgoing_mobility_request_.publish(request); - if(!request_sent){ - request_sent_time = ros::Time::now(); - request_sent = true; - } - cav_msgs::LaneChangeStatus lc_status_msg; - lc_status_msg.status = cav_msgs::LaneChangeStatus::PLAN_SENT; - lc_status_msg.description = "Requested lane merge"; - lanechange_status_pub_.publish(lc_status_msg); - } - - //if ack mobility response, send lanechange response - if(!negotiate || is_lanechange_accepted_){ - ROS_DEBUG_STREAM("negotiate:" << negotiate); - ROS_DEBUG_STREAM("is_lanechange_accepted:" << is_lanechange_accepted_); - - ROS_DEBUG_STREAM("Adding to response"); - add_maneuver_to_response(req,resp,planned_trajectory_points); - - } - else{ - if(!negotiate && !request_sent){ - request_sent_time = ros::Time::now(); - request_sent = true; - } - ros::Time planning_end_time = ros::Time::now(); - ros::Duration passed_time = planning_end_time - request_sent_time; - if(passed_time.toSec() >= lanechange_time_out_){ - cav_msgs::LaneChangeStatus lc_status_msg; - lc_status_msg.status = cav_msgs::LaneChangeStatus::TIMED_OUT; - lc_status_msg.description = "Request timed out for lane merge"; - lanechange_status_pub_.publish(lc_status_msg); - request_sent = false; //Reset variable - } - - } - //@SONAR_START@ - return true; - } - - void CooperativeLaneChangePlugin::add_maneuver_to_response(cav_srvs::PlanTrajectoryRequest &req, cav_srvs::PlanTrajectoryResponse &resp, std::vector &planned_trajectory_points){ - cav_msgs::TrajectoryPlan trajectory_plan; - trajectory_plan.header.frame_id = "map"; - trajectory_plan.header.stamp = ros::Time::now(); - trajectory_plan.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); - - trajectory_plan.trajectory_points = planned_trajectory_points; - trajectory_plan.initial_longitudinal_velocity = std::max(req.vehicle_state.longitudinal_vel, minimum_speed_); - resp.trajectory_plan = trajectory_plan; - - resp.related_maneuvers.push_back(req.maneuver_index_to_plan); - - resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); - } - - cav_msgs::MobilityRequest CooperativeLaneChangePlugin::create_mobility_request(std::vector& trajectory_plan, cav_msgs::Maneuver& maneuver){ - - cav_msgs::MobilityRequest request_msg; - cav_msgs::MobilityHeader header; - header.sender_id = sender_id_; - header.recipient_id = DEFAULT_STRING_; - header.sender_bsm_id = bsmIDtoString(bsm_core_); - header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); - clc_request_id_ = header.plan_id; - header.timestamp = trajectory_plan.front().target_time.toNSec() *1000000; - request_msg.m_header = header; - - request_msg.strategy = "carma/cooperative-lane-change"; - request_msg.plan_type.type = cav_msgs::PlanType::CHANGE_LANE_LEFT; - //Urgency- Currently unassigned - int urgency; - if(maneuver_fraction_completed_ <= starting_fraction_){ - urgency = 10; - } - else if(maneuver_fraction_completed_ <= mid_fraction_ ){ - urgency = 5; - } - else{ - urgency=1; - } - ROS_DEBUG_STREAM("Maneuver fraction completed:"<& traj_points) const{ - cav_msgs::Trajectory traj; - - cav_msgs::LocationECEF ecef_location = trajectory_point_to_ecef(traj_points[0]); - - if (traj_points.size()<2){ - ROS_WARN("Received Trajectory Plan is too small"); - traj.offsets = {}; - } - else{ - cav_msgs::LocationECEF prev_point = ecef_location; - for (size_t i=1; iprojectECEF({traj_point.x, traj_point.y, 0.0}, 1); - location.ecef_x = ecef_point.x() * 100.0; // Convert cm to m - location.ecef_y = ecef_point.y() * 100.0; - location.ecef_z = ecef_point.z() * 100.0; - - return location; - } - - std::vector CooperativeLaneChangePlugin::plan_lanechange(cav_srvs::PlanTrajectoryRequest &req){ - - lanelet::BasicPoint2d veh_pos(req.vehicle_state.x_pos_global, req.vehicle_state.y_pos_global); - double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack; - - // Only plan the trajectory for the requested LANE_CHANGE maneuver - std::vector maneuver_plan; - if(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan].type != cav_msgs::Maneuver::LANE_CHANGE) - { - throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type"); - } - maneuver_plan.push_back(req.maneuver_plan.maneuvers[req.maneuver_index_to_plan]); - - if(current_downtrack >= maneuver_plan.front().lane_change_maneuver.end_dist){ - request_sent = false; - } - basic_autonomy::waypoint_generation::DetailedTrajConfig wpg_detail_config; - basic_autonomy::waypoint_generation::GeneralTrajConfig wpg_general_config; - - wpg_general_config = basic_autonomy::waypoint_generation::compose_general_trajectory_config("cooperative_lanechange", downsample_ratio_, turn_downsample_ratio_); - - wpg_detail_config = basic_autonomy::waypoint_generation::compose_detailed_trajectory_config(trajectory_time_length_, - curve_resample_step_size_, minimum_speed_, - max_accel_, lateral_accel_limit_, - speed_moving_average_window_size_, - curvature_moving_average_window_size_, back_distance_, - buffer_ending_downtrack_); - - ROS_DEBUG_STREAM("Current downtrack:"< trajectory_points = basic_autonomy::waypoint_generation::compose_lanechange_trajectory_from_path(downsampled_points, req.vehicle_state, req.header.stamp, - wm_, ending_state_before_buffer_, wpg_detail_config); - ROS_DEBUG_STREAM("Compose Trajectory size:"<(msg->data.c_str()); // Build projector from proj string - } - -} diff --git a/cooperative_lanechange/src/cooperative_lanechange_node.cpp b/cooperative_lanechange/src/cooperative_lanechange_node.cpp new file mode 100644 index 0000000000..6b776f2256 --- /dev/null +++ b/cooperative_lanechange/src/cooperative_lanechange_node.cpp @@ -0,0 +1,661 @@ +/* + * 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 "cooperative_lanechange/cooperative_lanechange_node.hpp" + +namespace cooperative_lanechange +{ + namespace std_ph = std::placeholders; + + CooperativeLaneChangePlugin::CooperativeLaneChangePlugin(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::TacticalPlugin(options) + { + // Create initial config + config_ = Config(); + + // Declare parameters + config_.trajectory_time_length = declare_parameter("trajectory_time_length", config_.trajectory_time_length); + config_.control_plugin_name = declare_parameter("control_plugin_name", config_.control_plugin_name); + config_.minimum_speed = declare_parameter("minimum_speed", config_.minimum_speed); + config_.max_accel = declare_parameter("max_accel", config_.max_accel); + config_.minimum_lookahead_distance = declare_parameter("minimum_lookahead_distance", config_.minimum_lookahead_distance); + config_.maximum_lookahead_distance = declare_parameter("maximum_lookahead_distance", config_.maximum_lookahead_distance); + config_.minimum_lookahead_speed = declare_parameter("minimum_lookahead_speed", config_.minimum_lookahead_speed); + config_.maximum_lookahead_speed = declare_parameter("maximum_lookahead_speed", config_.maximum_lookahead_speed); + config_.lateral_accel_limit = declare_parameter("lateral_accel_limit", config_.lateral_accel_limit); + config_.speed_moving_average_window_size = declare_parameter("speed_moving_average_window_size", config_.speed_moving_average_window_size); + config_.curvature_moving_average_window_size = declare_parameter("curvature_moving_average_window_size", config_.curvature_moving_average_window_size); + config_.curvature_calc_lookahead_count = declare_parameter("curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count); + config_.downsample_ratio = declare_parameter("downsample_ratio", config_.downsample_ratio); + config_.destination_range = declare_parameter("destination_range", config_.destination_range); + config_.lanechange_time_out = declare_parameter("lanechange_time_out", config_.lanechange_time_out); + config_.min_timestep = declare_parameter("min_timestep", config_.min_timestep); + config_.starting_downtrack_range = declare_parameter("starting_downtrack_range", config_.starting_downtrack_range); + config_.starting_fraction = declare_parameter("starting_fraction", config_.starting_fraction); + config_.mid_fraction = declare_parameter("mid_fraction", config_.mid_fraction); + config_.min_desired_gap = declare_parameter("min_desired_gap", config_.min_desired_gap); + config_.desired_time_gap = declare_parameter("desired_time_gap", config_.desired_time_gap); + config_.turn_downsample_ratio = declare_parameter("turn_downsample_ratio", config_.turn_downsample_ratio); + config_.curve_resample_step_size = declare_parameter("curve_resample_step_size", config_.curve_resample_step_size); + config_.back_distance = declare_parameter("back_distance", config_.back_distance); + config_.buffer_ending_downtrack = declare_parameter("buffer_ending_downtrack", config_.buffer_ending_downtrack); + config_.vehicle_id = declare_parameter("vehicle_id", config_.vehicle_id); + } + + rcl_interfaces::msg::SetParametersResult CooperativeLaneChangePlugin::parameter_update_callback(const std::vector ¶meters) + { + auto error = update_params( + {{"control_plugin_name", config_.control_plugin_name}, + {"vehicle_id", config_.vehicle_id}}, parameters); + + auto error_2 = update_params( + {{"trajectory_time_length", config_.trajectory_time_length}, + {"minimum_speed", config_.minimum_speed}, + {"max_accel", config_.max_accel}, + {"minimum_lookahead_distance", config_.minimum_lookahead_distance}, + {"maximum_lookahead_distance", config_.maximum_lookahead_distance}, + {"minimum_lookahead_speed", config_.minimum_lookahead_speed}, + {"maximum_lookahead_speed", config_.maximum_lookahead_speed}, + {"lateral_accel_limit", config_.lateral_accel_limit}, + {"destination_range", config_.destination_range}, + {"lanechange_time_out", config_.lanechange_time_out}, + {"min_timestep", config_.min_timestep}, + {"starting_downtrack_range", config_.starting_downtrack_range}, + {"starting_fraction", config_.starting_fraction}, + {"mid_fraction", config_.mid_fraction}, + {"min_desired_gap", config_.min_desired_gap}, + {"curve_resample_step_size", config_.curve_resample_step_size}, + {"back_distance", config_.back_distance}, + {"buffer_ending_downtrack", config_.buffer_ending_downtrack}, + {"desired_time_gap", config_.desired_time_gap}}, parameters); + + auto error_3 = update_params( + {{"speed_moving_average_window_size", config_.speed_moving_average_window_size}, + {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size}, + {"curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count}, + {"downsample_ratio", config_.downsample_ratio}, + {"turn_downsample_ratio", config_.turn_downsample_ratio}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error && !error_2 && !error_3; + + return result; + } + + carma_ros2_utils::CallbackReturn CooperativeLaneChangePlugin::on_configure_plugin() + { + RCLCPP_INFO_STREAM(get_logger(), "CooperativeLaneChangePlugin trying to configure"); + + // Reset config + config_ = Config(); + + // Load parameters + get_parameter("trajectory_time_length", config_.trajectory_time_length); + get_parameter("control_plugin_name", config_.control_plugin_name); + get_parameter("minimum_speed", config_.minimum_speed); + get_parameter("max_accel", config_.max_accel); + get_parameter("minimum_lookahead_distance", config_.minimum_lookahead_distance); + get_parameter("maximum_lookahead_distance", config_.maximum_lookahead_distance); + get_parameter("minimum_lookahead_speed", config_.minimum_lookahead_speed); + get_parameter("maximum_lookahead_speed", config_.maximum_lookahead_speed); + get_parameter("lateral_accel_limit", config_.lateral_accel_limit); + get_parameter("speed_moving_average_window_size", config_.speed_moving_average_window_size); + get_parameter("curvature_moving_average_window_size", config_.curvature_moving_average_window_size); + get_parameter("curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count); + get_parameter("downsample_ratio", config_.downsample_ratio); + get_parameter("destination_range", config_.destination_range); + get_parameter("lanechange_time_out", config_.lanechange_time_out); + get_parameter("min_timestep", config_.min_timestep); + get_parameter("starting_downtrack_range", config_.starting_downtrack_range); + get_parameter("starting_fraction", config_.starting_fraction); + get_parameter("mid_fraction", config_.mid_fraction); + get_parameter("min_desired_gap", config_.min_desired_gap); + get_parameter("desired_time_gap", config_.desired_time_gap); + get_parameter("turn_downsample_ratio", config_.turn_downsample_ratio); + get_parameter("curve_resample_step_size", config_.curve_resample_step_size); + get_parameter("back_distance", config_.back_distance); + get_parameter("buffer_ending_downtrack", config_.buffer_ending_downtrack); + get_parameter("vehicle_id", config_.vehicle_id); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&CooperativeLaneChangePlugin::parameter_update_callback, this, std_ph::_1)); + + RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_); + + // Setup subscribers + pose_sub_ = create_subscription("current_pose", 1, + std::bind(&CooperativeLaneChangePlugin::pose_cb, this, std_ph::_1)); + twist_sub_ = create_subscription("current_velocity", 1, + std::bind(&CooperativeLaneChangePlugin::twist_cb, this, std_ph::_1)); + incoming_mobility_response_sub_ = create_subscription("incoming_mobility_response", 1, + std::bind(&CooperativeLaneChangePlugin::mobilityresponse_cb, this, std_ph::_1)); + georeference_sub_ = create_subscription("georeference", 1, + std::bind(&CooperativeLaneChangePlugin::georeference_cb, this, std_ph::_1)); + bsm_sub_ = create_subscription("bsm_outbound", 1, + std::bind(&CooperativeLaneChangePlugin::bsm_cb, this, std_ph::_1)); + + // Setup publishers + outgoing_mobility_request_pub_ = create_publisher("outgoing_mobility_request", 5); // Rate from yield plugin + lanechange_status_pub_ = create_publisher("cooperative_lane_change_status", 10); + + // Initialize World Model + wm_ = get_world_model(); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + void CooperativeLaneChangePlugin::mobilityresponse_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg){ + //@SONAR_STOP@ + if (clc_called_ && clc_request_id_ == msg->m_header.plan_id) + { + carma_planning_msgs::msg::LaneChangeStatus lc_status_msg; + if(msg->is_accepted) + { + is_lanechange_accepted_ = true; + lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::ACCEPTANCE_RECEIVED; + lc_status_msg.description = "Received lane merge acceptance"; + } + else + { + is_lanechange_accepted_ = false; + lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REJECTION_RECEIVED; + lc_status_msg.description = "Received lane merge rejection"; + } + lanechange_status_pub_->publish(lc_status_msg); + //@SONAR_START@ + } + else + { + RCLCPP_DEBUG_STREAM(get_logger(), "received mobility response is not related to CLC"); + } + + } + + double CooperativeLaneChangePlugin::find_current_gap(long veh2_lanelet_id, double veh2_downtrack, carma_planning_msgs::msg::VehicleState& ego_state) const + { + //find downtrack distance between ego and lag vehicle + RCLCPP_DEBUG_STREAM(get_logger(), "entered find_current_gap"); + double current_gap = 0.0; + lanelet::BasicPoint2d ego_pos(ego_state.x_pos_global, ego_state.y_pos_global); + //double ego_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack; + + lanelet::LaneletMapConstPtr const_map(wm_->getMap()); + lanelet::ConstLanelet veh2_lanelet = const_map->laneletLayer.get(veh2_lanelet_id); + RCLCPP_DEBUG_STREAM(get_logger(), "veh2_lanelet id " << veh2_lanelet.id()); + + auto current_lanelets = lanelet::geometry::findNearest(const_map->laneletLayer, ego_pos, 10); + if(current_lanelets.size() == 0) + { + RCLCPP_WARN_STREAM(get_logger(), "Cannot find any lanelet in map!"); + return true; + } + lanelet::ConstLanelet current_lanelet = current_lanelets[0].second; + RCLCPP_DEBUG_STREAM(get_logger(), "current llt id " << current_lanelet.id()); + + //Create temporary route between the two vehicles + lanelet::ConstLanelet start_lanelet = veh2_lanelet; + lanelet::ConstLanelet end_lanelet = current_lanelet; + + auto map_graph = wm_->getMapRoutingGraph(); + RCLCPP_DEBUG_STREAM(get_logger(), "Graph created"); + + auto temp_route = map_graph->getRoute(start_lanelet, end_lanelet); + RCLCPP_DEBUG_STREAM(get_logger(), "Route created"); + + //Throw exception if there is no shortest path from veh2 to subject vehicle + lanelet::routing::LaneletPath shortest_path2; + if(temp_route) + { + shortest_path2 = temp_route.get().shortestPath(); + } + else{ + RCLCPP_ERROR_STREAM(get_logger(), "No path exists from roadway object to subject"); + throw std::invalid_argument("No path exists from roadway object to subject"); + } + + RCLCPP_DEBUG_STREAM(get_logger(), "Shorted path created size: " << shortest_path2.size()); + for (auto llt : shortest_path2) + { + RCLCPP_DEBUG_STREAM(get_logger(), "llt id route: " << llt.id()); + } + + //To find downtrack- creating temporary route from veh2 to veh1(ego vehicle) + double veh1_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack; + RCLCPP_DEBUG_STREAM(get_logger(), "ego_current_downtrack:" << veh1_current_downtrack); + + current_gap = veh1_current_downtrack - veh2_downtrack; + RCLCPP_DEBUG_STREAM(get_logger(), "Finding current gap"); + RCLCPP_DEBUG_STREAM(get_logger(), "Veh1 current downtrack: " << veh1_current_downtrack << " veh2 downtrack: " << veh2_downtrack); + + return current_gap; + } + + void CooperativeLaneChangePlugin::pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg) + { + pose_msg_ = *msg; + } + + void CooperativeLaneChangePlugin::twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg) + { + current_speed_ = msg->twist.linear.x; + } + + void CooperativeLaneChangePlugin::bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg) + { + bsm_core_ = msg->core_data; + } + + void CooperativeLaneChangePlugin::plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) + { + // Set boolean flag if this is the first time this service has been called + if (!clc_called_) + { + clc_called_ = true; + } + + // Only plan the trajectory for the requested LANE_CHANGE maneuver + std::vector maneuver_plan; + if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type != carma_planning_msgs::msg::Maneuver::LANE_CHANGE) + { + throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type"); + } + maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]); + + // Currently only checking for first lane change maneuver message + long target_lanelet_id = stol(maneuver_plan[0].lane_change_maneuver.ending_lane_id); + double target_downtrack = maneuver_plan[0].lane_change_maneuver.end_dist; + + // Get subject vehicle info + lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global); + double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack; + + RCLCPP_DEBUG_STREAM(get_logger(), "target_lanelet_id: " << target_lanelet_id); + RCLCPP_DEBUG_STREAM(get_logger(), "target_downtrack: " << target_downtrack); + RCLCPP_DEBUG_STREAM(get_logger(), "current_downtrack: " << current_downtrack); + RCLCPP_DEBUG_STREAM(get_logger(), "Starting CLC downtrack: " << maneuver_plan[0].lane_change_maneuver.start_dist); + + if(current_downtrack < maneuver_plan[0].lane_change_maneuver.start_dist - config_.starting_downtrack_range){ + RCLCPP_DEBUG_STREAM(get_logger(), "Lane change trajectory will not be planned. current_downtrack is more than " << config_.starting_downtrack_range << " meters before starting CLC downtrack"); + return; + } + auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, veh_pos, 10); + long current_lanelet_id = current_lanelets[0].second.id(); + if(current_lanelet_id == target_lanelet_id && current_downtrack >= target_downtrack - config_.destination_range){ + carma_planning_msgs::msg::LaneChangeStatus lc_status_msg; + lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::PLANNING_SUCCESS; + //No description as per UI documentation + lanechange_status_pub_->publish(lc_status_msg); + } + + long veh2_lanelet_id = 0; + double veh2_downtrack = 0.0, veh2_speed = 0.0; + bool foundRoadwayObject = false; + bool negotiate = true; + std::vector rwol = wm_->getRoadwayObjects(); + //Assuming only one connected vehicle in list + for(int i = 0; i < rwol.size(); i++){ + if(rwol[i].connected_vehicle_type.type == carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED){ + veh2_lanelet_id = rwol[0].lanelet_id; + veh2_downtrack = rwol[0].down_track; //Returns downtrack + veh2_speed = rwol[0].object.velocity.twist.linear.x; + foundRoadwayObject = true; + break; + } + } + if(foundRoadwayObject){ + RCLCPP_DEBUG_STREAM(get_logger(), "Found Roadway object"); + //get current_gap + RCLCPP_DEBUG_STREAM(get_logger(), "veh2_lanelet_id: " << veh2_lanelet_id << ", veh2_downtrack: " << veh2_downtrack); + + double current_gap = find_current_gap(veh2_lanelet_id, veh2_downtrack, req->vehicle_state); + RCLCPP_DEBUG_STREAM(get_logger(), "Current gap: " << current_gap); + + //get desired gap - desired time gap (default 3s)* relative velocity + double relative_velocity = current_speed_ - veh2_speed; + RCLCPP_DEBUG_STREAM(get_logger(), "Relative velocity: " << relative_velocity); + double desired_gap = config_.desired_time_gap * relative_velocity; + RCLCPP_DEBUG_STREAM(get_logger(), "Desired gap: " << desired_gap); + + if(desired_gap < config_.min_desired_gap){ + desired_gap = config_.min_desired_gap; + } + // TODO - this condition needs to be re-enabled after testing + // if(current_gap > desired_gap){ + // negotiate = false; //No need for negotiation + // } + + } + else{ + RCLCPP_DEBUG_STREAM(get_logger(), "No roadway object"); + negotiate = false; + } + + //plan lanechange without filling in response + RCLCPP_DEBUG_STREAM(get_logger(), "Planning lane change trajectory"); + + std::string maneuver_id = maneuver_plan[0].lane_change_maneuver.parameters.maneuver_id; + if (original_lc_maneuver_values_.find(maneuver_id) == original_lc_maneuver_values_.end()) { + // If this lane change maneuver ID is being received for this first time, store its original start_dist and starting_lane_id locally + RCLCPP_DEBUG_STREAM(get_logger(), "Received maneuver id " << maneuver_id << " for the first time"); + RCLCPP_DEBUG_STREAM(get_logger(), "Original start dist is " << maneuver_plan[0].lane_change_maneuver.start_dist); + RCLCPP_DEBUG_STREAM(get_logger(), "Original starting_lane_id is " << maneuver_plan[0].lane_change_maneuver.starting_lane_id); + + // Create LaneChangeManeuverOriginalValues object for this lane change maneuver and add it to original_lc_maneuver_values_ + LaneChangeManeuverOriginalValues original_lc_values; + original_lc_values.maneuver_id = maneuver_id; + original_lc_values.original_starting_lane_id = maneuver_plan[0].lane_change_maneuver.starting_lane_id; + original_lc_values.original_start_dist = maneuver_plan[0].lane_change_maneuver.start_dist; + + original_lc_maneuver_values_[maneuver_id] = original_lc_values; + } + else { + // If the vehicle has just started this lane change, store its initial velocity locally; this velocity will be maintained throughout the lane change + if (current_downtrack >= (original_lc_maneuver_values_[maneuver_id]).original_start_dist && !(original_lc_maneuver_values_[maneuver_id]).has_started) { + original_lc_maneuver_values_[maneuver_id].has_started = true; + original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed); + + RCLCPP_DEBUG_STREAM(get_logger(), "Lane change maneuver " << maneuver_id << " has started, maintaining speed (in m/s): " << + original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms << " throughout lane change"); + } + } + + std::vector planned_trajectory_points = plan_lanechange(req); + + if(negotiate){ + RCLCPP_DEBUG_STREAM(get_logger(), "Negotiating"); + //send mobility request + //Planning for first lane change maneuver + carma_v2x_msgs::msg::MobilityRequest request = create_mobility_request(planned_trajectory_points, maneuver_plan[0]); + outgoing_mobility_request_pub_->publish(request); + if(!request_sent_){ + request_sent_time_ = this->now(); + request_sent_ = true; + } + carma_planning_msgs::msg::LaneChangeStatus lc_status_msg; + lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::PLAN_SENT; + lc_status_msg.description = "Requested lane merge"; + lanechange_status_pub_->publish(lc_status_msg); + } + + //if ack mobility response, send lanechange response + if(!negotiate || is_lanechange_accepted_){ + RCLCPP_DEBUG_STREAM(get_logger(), "negotiate:" << negotiate); + RCLCPP_DEBUG_STREAM(get_logger(), "is_lanechange_accepted:" << is_lanechange_accepted_); + + RCLCPP_DEBUG_STREAM(get_logger(), "Adding to response"); + add_trajectory_to_response(req,resp,planned_trajectory_points); + + } + else{ + if(!negotiate && !request_sent_){ + request_sent_time_ = this->now(); + request_sent_ = true; + } + rclcpp::Time planning_end_time = this->now(); + rclcpp::Duration passed_time = planning_end_time - request_sent_time_; + if(passed_time.seconds() >= config_.lanechange_time_out){ + carma_planning_msgs::msg::LaneChangeStatus lc_status_msg; + lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::TIMED_OUT; + lc_status_msg.description = "Request timed out for lane merge"; + lanechange_status_pub_->publish(lc_status_msg); + request_sent_ = false; //Reset variable + } + } + + return; + } + + void CooperativeLaneChangePlugin::add_trajectory_to_response(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, + const std::vector& planned_trajectory_points) + { + carma_planning_msgs::msg::TrajectoryPlan trajectory_plan; + trajectory_plan.header.frame_id = "map"; + trajectory_plan.header.stamp = this->now(); + trajectory_plan.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); + + trajectory_plan.trajectory_points = planned_trajectory_points; + trajectory_plan.initial_longitudinal_velocity = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed); + resp->trajectory_plan = trajectory_plan; + + resp->related_maneuvers.push_back(req->maneuver_index_to_plan); + + resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); + } + + carma_v2x_msgs::msg::MobilityRequest CooperativeLaneChangePlugin::create_mobility_request(std::vector& trajectory_plan, carma_planning_msgs::msg::Maneuver& maneuver) + { + carma_v2x_msgs::msg::MobilityRequest request_msg; + carma_v2x_msgs::msg::MobilityHeader header; + header.sender_id = config_.vehicle_id; + header.recipient_id = DEFAULT_STRING_; + header.sender_bsm_id = bsmIDtoString(bsm_core_); + header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()()); + clc_request_id_ = header.plan_id; + header.timestamp = rclcpp::Time(trajectory_plan.front().target_time).nanoseconds() * 1000000; + request_msg.m_header = header; + + request_msg.strategy = "carma/cooperative-lane-change"; + request_msg.plan_type.type = carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT; + + //Urgency- Currently unassigned + int urgency; + if(maneuver_fraction_completed_ <= config_.starting_fraction){ + urgency = 10; + } + else if(maneuver_fraction_completed_ <= config_.mid_fraction){ + urgency = 5; + } + else{ + urgency = 1; + } + RCLCPP_DEBUG_STREAM(get_logger(), "Maneuver fraction completed:"<& traj_points) const + { + carma_v2x_msgs::msg::Trajectory traj; + carma_v2x_msgs::msg::LocationECEF ecef_location = trajectory_point_to_ecef(traj_points[0]); + + if (traj_points.size() < 2){ + RCLCPP_WARN_STREAM(get_logger(), "Received Trajectory Plan is too small"); + traj.offsets = {}; + } + else{ + carma_v2x_msgs::msg::LocationECEF prev_point = ecef_location; + for (size_t i = 1; i < traj_points.size(); i++){ + + carma_v2x_msgs::msg::LocationOffsetECEF offset; + carma_v2x_msgs::msg::LocationECEF new_point = trajectory_point_to_ecef(traj_points[i]); // m to cm to fit the msg standard + offset.offset_x = (int16_t)(new_point.ecef_x - prev_point.ecef_x); + offset.offset_y = (int16_t)(new_point.ecef_y - prev_point.ecef_y); + offset.offset_z = (int16_t)(new_point.ecef_z - prev_point.ecef_z); + prev_point = new_point; + traj.offsets.push_back(offset); + } + } + + traj.location = ecef_location; + + return traj; + } + + carma_v2x_msgs::msg::LocationECEF CooperativeLaneChangePlugin::trajectory_point_to_ecef(const carma_planning_msgs::msg::TrajectoryPlanPoint& traj_point) const + { + if (!map_projector_) { + throw std::invalid_argument("No map projector available for ecef conversion"); + } + carma_v2x_msgs::msg::LocationECEF location; + + lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({traj_point.x, traj_point.y, 0.0}, 1); + location.ecef_x = ecef_point.x() * 100.0; // Convert cm to m + location.ecef_y = ecef_point.y() * 100.0; + location.ecef_z = ecef_point.z() * 100.0; + + return location; + } + + std::vector CooperativeLaneChangePlugin::plan_lanechange(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req) + { + lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global); + double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack; + + // Only plan the trajectory for the requested LANE_CHANGE maneuver + std::vector maneuver_plan; + if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type != carma_planning_msgs::msg::Maneuver::LANE_CHANGE) { + throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type"); + } + maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]); + + if(current_downtrack >= maneuver_plan.front().lane_change_maneuver.end_dist){ + request_sent_ = false; + } + + basic_autonomy::waypoint_generation::DetailedTrajConfig wpg_detail_config; + basic_autonomy::waypoint_generation::GeneralTrajConfig wpg_general_config; + + wpg_general_config = basic_autonomy::waypoint_generation::compose_general_trajectory_config("cooperative_lanechange", config_.downsample_ratio, config_.turn_downsample_ratio); + + wpg_detail_config = basic_autonomy::waypoint_generation::compose_detailed_trajectory_config(config_.trajectory_time_length, + config_.curve_resample_step_size, config_.minimum_speed, + config_.max_accel, config_.lateral_accel_limit, + config_.speed_moving_average_window_size, + config_.curvature_moving_average_window_size, config_.back_distance, + config_.buffer_ending_downtrack); + + RCLCPP_DEBUG_STREAM(get_logger(), "Current downtrack: " << current_downtrack); + + std::string maneuver_id = maneuver_plan.front().lane_change_maneuver.parameters.maneuver_id; + double original_start_dist = current_downtrack; // Initialize so original_start_dist cannot be less than the current downtrack + + if (original_lc_maneuver_values_.find(maneuver_id) != original_lc_maneuver_values_.end()) { + // Obtain the original start_dist associated with this lane change maneuver + original_start_dist = original_lc_maneuver_values_[maneuver_id].original_start_dist; + RCLCPP_DEBUG_STREAM(get_logger(), "Maneuver id " << maneuver_id << " original start_dist is " << original_start_dist); + + // Set this maneuver's starting_lane_id to the original starting_lane_id associated with this lane change maneuver + maneuver_plan.front().lane_change_maneuver.starting_lane_id = original_lc_maneuver_values_[maneuver_id].original_starting_lane_id; + RCLCPP_DEBUG_STREAM(get_logger(), "Updated maneuver id " << maneuver_id << " starting_lane_id to its original value of " << original_lc_maneuver_values_[maneuver_id].original_starting_lane_id); + + // If the vehicle has started this lane change, set the request's vehicle_state.longitudinal_vel to the velocity that the vehicle began this lane change at + if(original_lc_maneuver_values_[maneuver_id].has_started) { + req->vehicle_state.longitudinal_vel = original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms; + RCLCPP_DEBUG_STREAM(get_logger(), "Updating vehicle_state.longitudinal_vel to the initial lane change value of " << original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms); + } + } + else { + RCLCPP_WARN_STREAM(get_logger(), "No original values for lane change maneuver were found!"); + } + + double starting_downtrack = std::min(current_downtrack, original_start_dist); + + auto points_and_target_speeds = basic_autonomy::waypoint_generation::create_geometry_profile(maneuver_plan, starting_downtrack, wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config); + + // Calculate maneuver fraction completed (current_downtrack/(ending_downtrack-starting_downtrack) + auto maneuver_end_dist = maneuver_plan.back().lane_change_maneuver.end_dist; + auto maneuver_start_dist = maneuver_plan.front().lane_change_maneuver.start_dist; + maneuver_fraction_completed_ = (maneuver_start_dist - current_downtrack)/(maneuver_end_dist - maneuver_start_dist); + + RCLCPP_DEBUG_STREAM(get_logger(), "Maneuvers to points size: " << points_and_target_speeds.size()); + auto downsampled_points = carma_ros2_utils::containers::downsample_vector(points_and_target_speeds, config_.downsample_ratio); + + std::vector trajectory_points = basic_autonomy::waypoint_generation::compose_lanechange_trajectory_from_path(downsampled_points, req->vehicle_state, req->header.stamp, + wm_, ending_state_before_buffer_, wpg_detail_config); + RCLCPP_DEBUG_STREAM(get_logger(), "Compose Trajectory size: " << trajectory_points.size()); + return trajectory_points; + } + + void CooperativeLaneChangePlugin::georeference_cb(const std_msgs::msg::String::UniquePtr msg) + { + map_projector_ = std::make_shared(msg->data.c_str()); // Build projector from proj string + } + + std::string CooperativeLaneChangePlugin::bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core) + { + std::string res = ""; + for (size_t i = 0; i < bsm_core.id.size(); i++){ + res += std::to_string(bsm_core.id[i]); + } + return res; + } + + bool CooperativeLaneChangePlugin::get_availability() { + return true; + } + + std::string CooperativeLaneChangePlugin::get_version_id() { + return "v4.0"; // Version ID matches the value set in this package's package.xml + } + +} // cooperative_lanechange + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(cooperative_lanechange::CooperativeLaneChangePlugin) diff --git a/cooperative_lanechange/src/main.cpp b/cooperative_lanechange/src/main.cpp index c1aab1aa1b..4e8746197d 100644 --- a/cooperative_lanechange/src/main.cpp +++ b/cooperative_lanechange/src/main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -14,13 +14,20 @@ * the License. */ -#include -#include "cooperative_lanechange.h" +#include +#include "cooperative_lanechange/cooperative_lanechange_node.hpp" -int main(int argc, char** argv) +int main(int argc, char **argv) { - ros::init(argc, argv, "cooperative_lanechange"); - cooperative_lanechange::CooperativeLaneChangePlugin node; - node.run(); - return 0; -}; \ No newline at end of file + 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/cooperative_lanechange/test/cooperative_lanechange.test b/cooperative_lanechange/test/cooperative_lanechange.test deleted file mode 100644 index 2ffec8ca08..0000000000 --- a/cooperative_lanechange/test/cooperative_lanechange.test +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - \ No newline at end of file diff --git a/cooperative_lanechange/test/test_cooperative_plugin.cpp b/cooperative_lanechange/test/test_cooperative_plugin.cpp index ca53f4fe4f..87b469d1e4 100644 --- a/cooperative_lanechange/test/test_cooperative_plugin.cpp +++ b/cooperative_lanechange/test/test_cooperative_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -14,64 +14,59 @@ * the License. */ -#include "cooperative_lanechange.h" #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include + +#include +#include +#include + #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - TEST(CooperativeLaneChangePlugin,TestTrajectorytoecef) - { - cooperative_lanechange::CooperativeLaneChangePlugin worker; +#include - std::vector trajectory_plan; +#include "cooperative_lanechange/cooperative_lanechange_node.hpp" - std::string base_proj = "+proj=tmerc +lat_0=0.0 +lon_0=0.0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m " - "+no_defs"; - std_msgs::String msg; +namespace cooperative_lanechange +{ + TEST(CooperativeLaneChangePlugin, TestTrajectorytoecef){ + rclcpp::NodeOptions options; + auto worker = std::make_shared(options); + + worker->configure(); //Call configure state transition + worker->activate(); //Call activate state transition to get not read for runtime + + std::vector trajectory_plan; + + std::string base_proj = "+proj=tmerc +lat_0=0.0 +lon_0=0.0 +k=1 +x_0=0 +y_0=0 +datum=WGS84 +units=m +vunits=m +no_defs"; + std_msgs::msg::String msg; msg.data = base_proj; - std_msgs::StringConstPtr msg_ptr(new std_msgs::String(msg)); - worker.georeference_callback(msg_ptr); // Set projection + std::unique_ptr msg_ptr = std::make_unique(msg); + worker->georeference_cb(std::move(msg_ptr)); // Set projection - cav_msgs::TrajectoryPlanPoint point_1; + carma_planning_msgs::msg::TrajectoryPlanPoint point_1; point_1.x = 20.0; point_1.y = 0.0; trajectory_plan.push_back(point_1); - cav_msgs::LocationECEF ecef_point_1 = worker.trajectory_point_to_ecef(point_1); + carma_v2x_msgs::msg::LocationECEF ecef_point_1 = worker->trajectory_point_to_ecef(point_1); ASSERT_NEAR(ecef_point_1.ecef_x, 637813699.0, 0.001); ASSERT_NEAR(ecef_point_1.ecef_y, 1999.0, 0.001); ASSERT_NEAR(ecef_point_1.ecef_z, 0.0, 0.001); - + - cav_msgs::TrajectoryPlanPoint point_2; + carma_planning_msgs::msg::TrajectoryPlanPoint point_2; point_2.x = 19.0; point_2.y = 0.0; trajectory_plan.push_back(point_2); - ecef_point_1 = worker.trajectory_point_to_ecef(point_2); + ecef_point_1 = worker->trajectory_point_to_ecef(point_2); - cav_msgs::Trajectory traj = worker.trajectory_plan_to_trajectory(trajectory_plan); + carma_v2x_msgs::msg::Trajectory traj = worker->trajectory_plan_to_trajectory(trajectory_plan); ASSERT_NEAR(traj.location.ecef_x, 637813699.0, 0.001); ASSERT_NEAR(traj.location.ecef_y, 1999.0, 0.001); ASSERT_NEAR(traj.location.ecef_z, 0.0, 0.001); @@ -80,16 +75,15 @@ ASSERT_NEAR(traj.offsets[0].offset_x, 0.0, 0.001); ASSERT_NEAR(traj.offsets[0].offset_y, -100.0, 0.001); ASSERT_NEAR(traj.offsets[0].offset_z, 0.0, 0.001); - - } TEST(CooperativeLaneChangePlugin,TestLaneChangefunctions) { - // File to process. - std::string path = ros::package::getPath("basic_autonomy"); + // File to process. + std::string path = ament_index_cpp::get_package_share_directory("basic_autonomy_ros2"); std::string file = "/resource/map/town01_vector_map_lane_change.osm"; file = path.append(file); + lanelet::Id start_id = 107; lanelet::Id lane_change_start_id = 106; lanelet::Id lag_veh_start_id = 101; @@ -97,27 +91,33 @@ int projector_type = 0; std::string target_frame; lanelet::ErrorMessages load_errors; + // Parse geo reference info from the original lanelet map (.osm) lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame); lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors); + if (map->laneletLayer.size() == 0) { FAIL() << "Input map does not contain any lanelets"; } - std::shared_ptr cmw=std::make_shared(); + std::shared_ptr cmw = std::make_shared(); cmw->carma_wm::CARMAWorldModel::setMap(map); + //Set Route carma_wm::test::setRouteByIds({start_id,end_id},cmw); cmw->carma_wm::CARMAWorldModel::setMap(map); //get starting position - auto llt=map.get()->laneletLayer.get(start_id); + auto llt = map.get()->laneletLayer.get(start_id); lanelet::BasicPoint2d curr_pose = llt.centerline2d().front(); //Define arguments for function maneuvers_to_points - cooperative_lanechange::CooperativeLaneChangePlugin worker; - ros::Time::init(); //Initialize ros::Time + rclcpp::NodeOptions options; + auto worker = std::make_shared(options); + + worker->configure(); //Call configure state transition + worker->activate(); //Call activate state transition to get not read for runtime //get ending downtrack from lanelet id double ending_downtrack; @@ -127,84 +127,90 @@ double starting_downtrack = cmw->routeTrackPos(veh_pos).downtrack; ending_downtrack = cmw->routeTrackPos(shortest_path.back().centerline2d().back()).downtrack; - worker.wm_ = cmw; + worker->wm_ = cmw; //Define lane change maneuver - cav_msgs::Maneuver maneuver; - maneuver.type=cav_msgs::Maneuver::LANE_CHANGE; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE; maneuver.lane_change_maneuver.start_dist = starting_downtrack; maneuver.lane_change_maneuver.end_dist = ending_downtrack; maneuver.lane_change_maneuver.start_speed = 5.0; - maneuver.lane_change_maneuver.start_time = ros::Time::now(); + maneuver.lane_change_maneuver.start_time = worker->now(); //calculate end_time assuming constant acceleration - double acc = pow(maneuver.lane_change_maneuver.start_speed,2)/(2*(ending_downtrack - starting_downtrack)); - double end_time = maneuver.lane_change_maneuver.start_speed/acc; + double acc = pow(maneuver.lane_change_maneuver.start_speed, 2) / (2 * (ending_downtrack - starting_downtrack)); + double end_time = maneuver.lane_change_maneuver.start_speed / acc; maneuver.lane_change_maneuver.end_speed = 25.0; - maneuver.lane_change_maneuver.end_time = ros::Time(end_time + 10.0); + maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration(10 * 1e9); maneuver.lane_change_maneuver.starting_lane_id = std::to_string(lane_change_start_id); maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); - - std::vector maneuvers; + + std::vector maneuvers; maneuvers.push_back(maneuver); - worker.current_speed_ = maneuver.lane_change_maneuver.start_speed; - cav_msgs::VehicleState vehicle_state; + worker->current_speed_ = maneuver.lane_change_maneuver.start_speed; + carma_planning_msgs::msg::VehicleState vehicle_state; vehicle_state.x_pos_global = veh_pos.x(); vehicle_state.y_pos_global = veh_pos.y(); - - + /* Test plan lanechange */ - cav_srvs::PlanTrajectoryRequest req; - cav_srvs::PlanTrajectoryResponse resp; - - ros::Time::init(); - req.maneuver_plan.planning_start_time = ros::Time::now(); - req.maneuver_plan.planning_completion_time = req.maneuver_plan.planning_start_time + ros::Duration(10.0); + carma_planning_msgs::srv::PlanTrajectory::Request req; + carma_planning_msgs::srv::PlanTrajectory::Response resp; + + req.maneuver_plan.planning_start_time = worker->now(); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); req.vehicle_state.x_pos_global = veh_pos.x(); req.vehicle_state.y_pos_global = veh_pos.y(); req.vehicle_state.longitudinal_vel = maneuver.lane_change_maneuver.start_speed; - std::vector maneuvers_msg; + std::vector maneuvers_msg; //Define lane change maneuver - + maneuvers_msg.push_back(maneuver); req.maneuver_plan.maneuvers = maneuvers_msg; - std::vector traj_plan = worker.plan_lanechange(req); + auto req_ptr = std::make_shared(req); + std::vector traj_plan = worker->plan_lanechange(req_ptr); EXPECT_TRUE(traj_plan.size() > 2); - cav_msgs::MobilityRequest request = worker.create_mobility_request(traj_plan, maneuver); - EXPECT_EQ(cav_msgs::PlanType::CHANGE_LANE_LEFT, request.plan_type.type); + carma_v2x_msgs::msg::MobilityRequest request = worker->create_mobility_request(traj_plan, maneuver); + EXPECT_EQ(carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT, request.plan_type.type); /*Test compose trajectort and helper function*/ - std::vector trajectory; - + std::vector trajectory; } TEST(CooperativeLaneChangePlugin,TestAddManeuvertoResponse){ - cooperative_lanechange::CooperativeLaneChangePlugin worker; - cav_srvs::PlanTrajectoryRequest req; - cav_srvs::PlanTrajectoryResponse resp; + rclcpp::NodeOptions options; + auto worker = std::make_shared(options); + + worker->configure(); //Call configure state transition + worker->activate(); //Call activate state transition to get not read for runtime + + carma_planning_msgs::srv::PlanTrajectory::Request req; + carma_planning_msgs::srv::PlanTrajectory::Response resp; req.maneuver_index_to_plan = 0; - ros::Time::init(); - req.maneuver_plan.planning_start_time = ros::Time::now(); - req.maneuver_plan.planning_completion_time = req.maneuver_plan.planning_start_time + ros::Duration(10.0); + req.maneuver_plan.planning_start_time = worker->now(); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); lanelet::BasicPoint2d veh_pos(1.0,1.0); req.vehicle_state.x_pos_global = veh_pos.x(); req.vehicle_state.y_pos_global = veh_pos.y(); req.vehicle_state.longitudinal_vel = 10.0; - std::vector planned_trajectory ={}; - worker.add_maneuver_to_response(req,resp,planned_trajectory); - EXPECT_EQ(10.0,resp.trajectory_plan.initial_longitudinal_velocity); - EXPECT_EQ(0, resp.related_maneuvers.back()); + std::vector planned_trajectory = {}; + + auto req_ptr = std::make_shared(req); + auto resp_ptr = std::make_shared(resp); + worker->add_trajectory_to_response(req_ptr, resp_ptr, planned_trajectory); + EXPECT_EQ(10.0, resp_ptr->trajectory_plan.initial_longitudinal_velocity); + EXPECT_EQ(0, resp_ptr->related_maneuvers.back()); } TEST(CooperativeLaneChangePlugin,Testcurrentgapcb){ // File to process. - std::string path = ros::package::getPath("basic_autonomy"); - std::string file = "/resource/map/town01_vector_map_lane_change.osm"; + std::string path = ament_index_cpp::get_package_share_directory("basic_autonomy_ros2"); + std::string file = "/resource/map/town01_vector_map_lane_change.osm"; file = path.append(file); + lanelet::Id start_id = 107; lanelet::Id lane_change_start_id = 106; lanelet::Id lag_veh_start_id = 101; @@ -230,9 +236,13 @@ auto llt=map.get()->laneletLayer.get(start_id); lanelet::BasicPoint2d curr_pose = llt.centerline2d().front(); + rclcpp::NodeOptions options; + auto worker = std::make_shared(options); + + worker->configure(); //Call configure state transition + worker->activate(); //Call activate state transition to get not read for runtime + //Define arguments for function maneuvers_to_points - cooperative_lanechange::CooperativeLaneChangePlugin worker; - ros::Time::init(); //Initialize ros::Time //get ending downtrack from lanelet id double ending_downtrack; @@ -242,65 +252,63 @@ double starting_downtrack = cmw->routeTrackPos(veh_pos).downtrack; ending_downtrack = cmw->routeTrackPos(shortest_path.back().centerline2d().back()).downtrack; - worker.wm_ = cmw; + worker->wm_ = cmw; - //Define lane change maneuver - cav_msgs::Maneuver maneuver; - maneuver.type=cav_msgs::Maneuver::LANE_CHANGE; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE; maneuver.lane_change_maneuver.start_dist = starting_downtrack; maneuver.lane_change_maneuver.end_dist = ending_downtrack; maneuver.lane_change_maneuver.start_speed = 5.0; - maneuver.lane_change_maneuver.start_time = ros::Time::now(); + maneuver.lane_change_maneuver.start_time = worker->now(); //calculate end_time assuming constant acceleration double acc = pow(maneuver.lane_change_maneuver.start_speed,2)/(2*(ending_downtrack - starting_downtrack)); double end_time = maneuver.lane_change_maneuver.start_speed/acc; maneuver.lane_change_maneuver.end_speed = 25.0; - maneuver.lane_change_maneuver.end_time = ros::Time(end_time + 10.0); + maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration(10 * 1e9); maneuver.lane_change_maneuver.starting_lane_id = std::to_string(lane_change_start_id); maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); - std::vector maneuvers; + std::vector maneuvers; maneuvers.push_back(maneuver); - worker.current_speed_ = maneuver.lane_change_maneuver.start_speed; - cav_msgs::VehicleState vehicle_state; + worker->current_speed_ = maneuver.lane_change_maneuver.start_speed; + carma_planning_msgs::msg::VehicleState vehicle_state; vehicle_state.x_pos_global = veh_pos.x(); vehicle_state.y_pos_global = veh_pos.y(); - cav_srvs::PlanTrajectoryRequest req; - cav_srvs::PlanTrajectoryResponse resp; + carma_planning_msgs::srv::PlanTrajectory::Request req; + carma_planning_msgs::srv::PlanTrajectory::Response resp; - ros::Time::init(); - req.maneuver_plan.planning_start_time = ros::Time::now(); - req.maneuver_plan.planning_completion_time = req.maneuver_plan.planning_start_time + ros::Duration(10.0); + req.maneuver_plan.planning_start_time = worker->now(); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); req.vehicle_state.x_pos_global = veh_pos.x(); req.vehicle_state.y_pos_global = veh_pos.y(); req.vehicle_state.longitudinal_vel = maneuver.lane_change_maneuver.start_speed; - std::vector maneuvers_msg; + std::vector maneuvers_msg; //Define lane change maneuver maneuvers_msg.push_back(maneuver); req.maneuver_plan.maneuvers = maneuvers_msg; // //Add roadway object - lag vehicle - cav_msgs::ExternalObject object; + carma_perception_msgs::msg::ExternalObject object; // //Test plan trajectory cb object.id =1; - object.object_type = cav_msgs::ExternalObject::SMALL_VEHICLE; + object.object_type = carma_perception_msgs::msg::ExternalObject::SMALL_VEHICLE; object.pose.pose.position.x =0.0; object.pose.pose.position.y =0.0; object.pose.pose.position.z =0.0; object.velocity.twist.linear.x = 5.0; - geometry_msgs::Vector3 size; + geometry_msgs::msg::Vector3 size; size.x = 4; size.y = 2; size.z = 1; object.size = size; - cav_msgs::PredictedState pred; + carma_perception_msgs::msg::PredictedState pred; auto pred_pose = object.pose.pose; pred_pose.position.y += 1; pred.predicted_position = pred_pose; @@ -308,27 +316,27 @@ object.predictions.push_back(pred); - cav_msgs::RoadwayObstacle obstacle; + carma_perception_msgs::msg::RoadwayObstacle obstacle; obstacle.object = object; - obstacle.connected_vehicle_type.type = cav_msgs::ConnectedVehicleType::CONNECTED; + obstacle.connected_vehicle_type.type = carma_perception_msgs::msg::ConnectedVehicleType::CONNECTED; obstacle.lanelet_id = lag_veh_start_id; obstacle.cross_track = 0.0; obstacle.down_track = 0.0; - std::vector obstacles; + std::vector obstacles; obstacles.push_back(obstacle); cmw->setRoadwayObjects(obstacles); cmw->setMap(map); - EXPECT_TRUE(worker.find_current_gap(obstacle.lanelet_id,obstacle.down_track, req.vehicle_state) > 0.0); + EXPECT_TRUE(worker->find_current_gap(obstacle.lanelet_id,obstacle.down_track, req.vehicle_state) > 0.0); } TEST(CooperativeLaneChangePlugin,TestNoPath_roadwayobject){ //Tests behavior when there is no path from roadway object to subject vehicle // File to process. - std::string path = ros::package::getPath("basic_autonomy"); - std::string file = "/resource/map/town01_vector_map_lane_change.osm"; + std::string path = ament_index_cpp::get_package_share_directory("basic_autonomy_ros2"); + std::string file = "/resource/map/town01_vector_map_lane_change.osm"; file = path.append(file); lanelet::Id start_id = 107; lanelet::Id lane_change_start_id = 106; @@ -355,9 +363,13 @@ auto llt=map.get()->laneletLayer.get(start_id); lanelet::BasicPoint2d curr_pose = llt.centerline2d().front(); + rclcpp::NodeOptions options; + auto worker = std::make_shared(options); + + worker->configure(); //Call configure state transition + worker->activate(); //Call activate state transition to get not read for runtime + //Define arguments for function maneuvers_to_points - cooperative_lanechange::CooperativeLaneChangePlugin worker; - ros::Time::init(); //Initialize ros::Time //get ending downtrack from lanelet id double ending_downtrack; @@ -368,65 +380,64 @@ double starting_downtrack = cmw->routeTrackPos(veh_pos).downtrack; ending_downtrack = cmw->routeTrackPos(shortest_path.back().centerline2d().back()).downtrack; - worker.wm_ = cmw; + worker->wm_ = cmw; //Define lane change maneuver - cav_msgs::Maneuver maneuver; - maneuver.type=cav_msgs::Maneuver::LANE_CHANGE; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_CHANGE; maneuver.lane_change_maneuver.start_dist = starting_downtrack; maneuver.lane_change_maneuver.end_dist = ending_downtrack; maneuver.lane_change_maneuver.start_speed = 5.0; - maneuver.lane_change_maneuver.start_time = ros::Time::now(); + maneuver.lane_change_maneuver.start_time = worker->now(); //calculate end_time assuming constant acceleration double acc = pow(maneuver.lane_change_maneuver.start_speed,2)/(2*(ending_downtrack - starting_downtrack)); double end_time = maneuver.lane_change_maneuver.start_speed/acc; maneuver.lane_change_maneuver.end_speed = 25.0; - maneuver.lane_change_maneuver.end_time = ros::Time(end_time + 10.0); + maneuver.lane_change_maneuver.end_time = rclcpp::Time(end_time) + rclcpp::Duration(10 * 1e9); maneuver.lane_change_maneuver.starting_lane_id = std::to_string(lane_change_start_id); maneuver.lane_change_maneuver.ending_lane_id = std::to_string(end_id); - std::vector maneuvers; + std::vector maneuvers; maneuvers.push_back(maneuver); - worker.current_speed_ = maneuver.lane_change_maneuver.start_speed; - cav_msgs::VehicleState vehicle_state; + worker->current_speed_ = maneuver.lane_change_maneuver.start_speed; + carma_planning_msgs::msg::VehicleState vehicle_state; vehicle_state.x_pos_global = veh_pos.x(); vehicle_state.y_pos_global = veh_pos.y(); - cav_srvs::PlanTrajectoryRequest req; - cav_srvs::PlanTrajectoryResponse resp; + carma_planning_msgs::srv::PlanTrajectory::Request req; + carma_planning_msgs::srv::PlanTrajectory::Response resp; - ros::Time::init(); - req.maneuver_plan.planning_start_time = ros::Time::now(); - req.maneuver_plan.planning_completion_time = req.maneuver_plan.planning_start_time + ros::Duration(10.0); + req.maneuver_plan.planning_start_time = worker->now(); + req.maneuver_plan.planning_completion_time = rclcpp::Time(req.maneuver_plan.planning_start_time) + rclcpp::Duration(10 * 1e9); req.vehicle_state.x_pos_global = veh_pos.x(); req.vehicle_state.y_pos_global = veh_pos.y(); req.vehicle_state.longitudinal_vel = maneuver.lane_change_maneuver.start_speed; - std::vector maneuvers_msg; + std::vector maneuvers_msg; //Define lane change maneuver maneuvers_msg.push_back(maneuver); req.maneuver_plan.maneuvers = maneuvers_msg; //Add roadway object - lag vehicle - cav_msgs::ExternalObject object; + carma_perception_msgs::msg::ExternalObject object; // //Test plan trajectory cb //Create a roadway object - Here we define an object from which there is no path to the subject object.id =1; - object.object_type = cav_msgs::ExternalObject::SMALL_VEHICLE; + object.object_type = carma_perception_msgs::msg::ExternalObject::SMALL_VEHICLE; object.pose.pose.position.x =0.0; object.pose.pose.position.y =0.0; object.pose.pose.position.z =0.0; object.velocity.twist.linear.x = 5.0; - geometry_msgs::Vector3 size; + geometry_msgs::msg::Vector3 size; size.x = 4; size.y = 2; size.z = 1; object.size = size; - cav_msgs::PredictedState pred; + carma_perception_msgs::msg::PredictedState pred; auto pred_pose = object.pose.pose; pred_pose.position.y += 1; pred.predicted_position = pred_pose; @@ -434,32 +445,39 @@ object.predictions.push_back(pred); - cav_msgs::RoadwayObstacle obstacle; + carma_perception_msgs::msg::RoadwayObstacle obstacle; obstacle.object = object; - obstacle.connected_vehicle_type.type = cav_msgs::ConnectedVehicleType::CONNECTED; + obstacle.connected_vehicle_type.type = carma_perception_msgs::msg::ConnectedVehicleType::CONNECTED; obstacle.lanelet_id = lag_veh_start_id; obstacle.cross_track = 0.0; obstacle.down_track = 0.0; - std::vector obstacles; + std::vector obstacles; obstacles.push_back(obstacle); cmw->setRoadwayObjects(obstacles); cmw->setMap(map); try{ - worker.find_current_gap(obstacle.lanelet_id,obstacle.down_track,req.vehicle_state); + worker->find_current_gap(obstacle.lanelet_id, obstacle.down_track, req.vehicle_state); } catch(std::exception &ex){ EXPECT_EQ(ex.what(), std::string("No path exists from roadway object to subject")); } } +} + +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(); - int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_cooperative_lanechange"); - auto res = RUN_ALL_TESTS(); - return res; - } \ No newline at end of file + return success; +} \ No newline at end of file diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 5eeac64353..842e394c8a 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -59,3 +59,4 @@ # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin + - /guidance/plugins/cooperative_lanechange From 7e28e939c6fa5b11209242055bd0d9861a4c30f7 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Wed, 24 Aug 2022 11:20:56 -0400 Subject: [PATCH 085/165] Feature/platooning tactical ros2 (#1905) This PR ports the platooning_tactical_plugin to ROS2. Should be merged in tandam with usdot-fhwa-stol/carma-config#208 Related Issue Resolves #1904 Motivation and Context ROS2 Ports How Has This Been Tested? Local integration testing verified all topic connections and state transitions. Unit tests all running and passing --- carma/launch/guidance.launch | 6 - carma/launch/plugins.launch.py | 22 +++ .../src/platoon_strategic_ihp_node.cpp | 10 +- platooning_tactical_plugin/CMakeLists.txt | 109 +++++--------- platooning_tactical_plugin/README.md | 9 ++ .../platooning_tactical_plugin.h | 49 +++--- .../platooning_tactical_plugin_config.h | 2 +- .../platooning_tactical_plugin_node.h | 89 +++++------ .../launch/platooning_tactical_plugin.launch | 19 --- .../platooning_tactical_plugin_launch.py | 68 +++++++++ platooning_tactical_plugin/package.xml | 67 +++++---- platooning_tactical_plugin/src/main.cpp | 33 ++++ .../src/platooning_tactical_plugin.cpp | 62 ++++---- .../src/platooning_tactical_plugin_node.cpp | 142 ++++++++++++++++-- platooning_tactical_plugin/test/node_test.cpp | 37 +++++ ...st_platooning_tactical_plugin_planning.cpp | 76 ++++------ .../config/guidance_controller_config.yaml | 3 +- 17 files changed, 499 insertions(+), 304 deletions(-) create mode 100644 platooning_tactical_plugin/README.md delete mode 100644 platooning_tactical_plugin/launch/platooning_tactical_plugin.launch create mode 100644 platooning_tactical_plugin/launch/platooning_tactical_plugin_launch.py create mode 100644 platooning_tactical_plugin/src/main.cpp create mode 100644 platooning_tactical_plugin/test/node_test.cpp diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 9e0e6d4f4b..891ba4b9aa 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -138,12 +138,6 @@ - - - - - - diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 01908e462f..20e569fd60 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -58,6 +58,9 @@ def generate_launch_description(): platoon_strategic_ihp_param_file = os.path.join( get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') + + platoon_tactical_ihp_param_file = os.path.join( + get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') @@ -202,6 +205,25 @@ def generate_launch_description(): platoon_strategic_ihp_param_file, vehicle_config_param_file ] + ), + ComposableNode( + package='platooning_tactical_plugin', + plugin='platooning_tactical_plugin::Node', + name='platooning_tactical_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('platooning_tactical_plugin', env_log_levels) } + ], + 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" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("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" ] ), + ], + parameters=[ platoon_tactical_ihp_param_file, vehicle_config_param_file ] ), ] ) diff --git a/platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp b/platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp index dd95638750..cadc2b3d20 100644 --- a/platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp +++ b/platooning_strategic_IHP/src/platoon_strategic_ihp_node.cpp @@ -102,16 +102,16 @@ namespace platoon_strategic_ihp // vehicle_length and id excluded since they should not be updated at runtime }, parameters); - if (worker_) - { - worker_->setConfig(config_); - } - rcl_interfaces::msg::SetParametersResult result; result.successful = !error && !error2 && !error3; + if (result.successful && worker_) + { + worker_->setConfig(config_); + } + return result; } diff --git a/platooning_tactical_plugin/CMakeLists.txt b/platooning_tactical_plugin/CMakeLists.txt index a8ff05dba0..d2c2efa4bd 100644 --- a/platooning_tactical_plugin/CMakeLists.txt +++ b/platooning_tactical_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ -# Copyright (C) 2020-2021 LEIDOS. +# 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 @@ -13,94 +13,61 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(platooning_tactical_plugin) +# Declare carma package and check ROS version find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -set( CATKIN_DEPS - roscpp - std_msgs - cav_msgs - cav_srvs - carma_utils - trajectory_utils - autoware_msgs - carma_wm - tf - tf2 - tf2_geometry_msgs - basic_autonomy -) - -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) - -## System dependencies are found with CMake's conventions -find_package(Boost) -find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost EIGEN3 -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -########### -## Build ## -########### +# Name build targets +set(node_exec platooning_tactical_plugin_node_exec) +set(node_lib platooning_tactical_plugin_node) +# Includes include_directories( include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} ) -add_executable( ${PROJECT_NAME} - src/platooning_tactical_plugin_node.cpp) - -add_library(platooning_tactical_plugin_library - src/platooning_tactical_plugin.cpp +# Build +ament_auto_add_library(${node_lib} SHARED + src/platooning_tactical_plugin_node.cpp + src/platooning_tactical_plugin.cpp ) -target_link_libraries(platooning_tactical_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(platooning_tactical_plugin_library ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} platooning_tactical_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${PROJECT_NAME} platooning_tactical_plugin_library ${catkin_EXPORTED_TARGETS}) +ament_auto_add_executable(${node_exec} + src/main.cpp +) -############# -## Install ## -############# +# Register component +rclcpp_components_register_nodes(${node_lib} "platooning_tactical_plugin::Node") -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -## Install C++ -install(TARGETS ${PROJECT_NAME} platooning_tactical_plugin_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# Testing +if(BUILD_TESTING) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(test_platooning_tactical_plugin test/node_test.cpp test/test_platooning_tactical_plugin_planning.cpp) + + ament_target_dependencies(test_platooning_tactical_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_platooning_tactical_plugin ${node_lib}) -############# -## Testing ## -############# +endif() -catkin_add_gmock(${PROJECT_NAME}-test - test/test_platooning_tactical_plugin_planning.cpp +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch ) -target_link_libraries(${PROJECT_NAME}-test platooning_tactical_plugin_library ${catkin_LIBRARIES}) diff --git a/platooning_tactical_plugin/README.md b/platooning_tactical_plugin/README.md new file mode 100644 index 0000000000..1944327b55 --- /dev/null +++ b/platooning_tactical_plugin/README.md @@ -0,0 +1,9 @@ +# platooning_tactical_plugin + +Tactical plugin for the Integrated Highway Prototype 2 (IHP2) implementation which allows platooning to include front and rear merge joins. Note despite the name, as of Aug 2022 testing for this IHP2 implementation was only up to 35mph on a closed test track per limitations of the CARMA Platform system. + +Overview +https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2138079233/Integrated+Highway+Prototype+2+IHP2 + +Design +https://usdot-carma.atlassian.net/wiki/spaces/CRMPLT/pages/2138767361/IHP2+Platooning+Plugin diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h index 8b767c0397..d7d19a3050 100644 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h +++ b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin.h @@ -1,7 +1,7 @@ #pragma once /*------------------------------------------------------------------------------ -* Copyright (C) 2020-2021 LEIDOS. +* Copyright (C) 2020-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 @@ -18,26 +18,22 @@ ------------------------------------------------------------------------------*/ #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include +#include #include "platooning_tactical_plugin_config.h" namespace platooning_tactical_plugin { -using PublishPluginDiscoveryCB = std::function; -using DebugPublisher = std::function; +using DebugPublisher = std::function; /** * \brief Convenience class for pairing 2d points with speeds */ @@ -57,12 +53,11 @@ class PlatooningTacticalPlugin /** * \brief Constructor * - * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data + * \param wm Pointer to initalized instance of the carma world model for accessing semantic map data * \param config The configuration to be used for this object - * \param plugin_discovery_publisher Callback which will publish the current plugin discovery state */ PlatooningTacticalPlugin(carma_wm::WorldModelConstPtr wm, PlatooningTacticalPluginConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher); + std::shared_ptr timer_factory); /** * \brief Service callback for trajectory planning @@ -72,27 +67,25 @@ class PlatooningTacticalPlugin * * \return True if success. False otherwise */ - bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp); + bool plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::Request& req, carma_planning_msgs::srv::PlanTrajectory::Response& resp); /** - * \brief Method to call at fixed rate in execution loop. Will publish plugin discovery updates - * - * \return True if the node should continue running. False otherwise + * \brief Set the current config */ - bool onSpin(); + void set_config(PlatooningTacticalPluginConfig config); - cav_msgs::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later + carma_planning_msgs::msg::VehicleState ending_state_before_buffer_; //state before applying extra points for curvature calculation that are removed later private: carma_wm::WorldModelConstPtr wm_; PlatooningTacticalPluginConfig config_; - PublishPluginDiscoveryCB plugin_discovery_publisher_; - cav_msgs::Plugin plugin_discovery_msg_; - carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_; + carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_; DebugPublisher debug_publisher_; - cav_msgs::VehicleState ending_state_before_buffer; //state before applying extra points for curvature calculation that are removed later + carma_planning_msgs::msg::VehicleState ending_state_before_buffer; //state before applying extra points for curvature calculation that are removed later + + std::shared_ptr timer_factory_; }; -}; // namespace platooning_tactical_plugin \ No newline at end of file +} // namespace platooning_tactical_plugin \ No newline at end of file diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h index f658c54aff..96545ad0fa 100644 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h +++ b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_config.h @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2020 LEIDOS. + * 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 diff --git a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h index 9e7f472e52..7d7902fdc3 100644 --- a/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h +++ b/platooning_tactical_plugin/include/platooning_tactical_plugin/platooning_tactical_plugin_node.h @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -16,72 +16,53 @@ * the License. */ -#include -#include -#include -#include #include - +#include +#include #include "platooning_tactical_plugin.h" #include "platooning_tactical_plugin_config.h" + namespace platooning_tactical_plugin { -/** - * \brief ROS node for the PlatooningTacticalPlugin - */ -class PlatooningTacticalPluginNode -{ -public: - /** - * \brief Entrypoint for this node + * \brief ROS node for the PlatooningTacticalPlugin */ - void run() + class Node : public carma_guidance_plugins::TacticalPlugin { - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - carma_wm::WMListener wml; - auto wm_ = wml.getWorldModel(); - - ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); - - PlatooningTacticalPluginConfig config; - - pnh.param("trajectory_time_length", config.trajectory_time_length, config.trajectory_time_length); - pnh.param("curve_resample_step_size", config.curve_resample_step_size, config.curve_resample_step_size); - pnh.param("default_downsample_ratio", config.default_downsample_ratio, config.default_downsample_ratio); - pnh.param("turn_downsample_ratio", config.turn_downsample_ratio, config.turn_downsample_ratio); - pnh.param("minimum_speed", config.minimum_speed, config.minimum_speed); - pnh.param("max_accel_multiplier", config.max_accel_multiplier, config.max_accel_multiplier); - pnh.param("lat_accel_multiplier", config.lat_accel_multiplier, config.lat_accel_multiplier); - pnh.param("back_distance", config.back_distance, config.back_distance); - pnh.param("speed_moving_average_window_size", config.speed_moving_average_window_size, - config.speed_moving_average_window_size); - pnh.param("curvature_moving_average_window_size", config.curvature_moving_average_window_size, - config.curvature_moving_average_window_size); - pnh.param("/vehicle_acceleration_limit", config.max_accel, config.max_accel); - pnh.param("/vehicle_lateral_accel_limit", config.lateral_accel_limit, config.lateral_accel_limit); - pnh.param("enable_object_avoidance", config.enable_object_avoidance, config.enable_object_avoidance); - pnh.param("buffer_ending_downtrack", config.buffer_ending_downtrack, config.buffer_ending_downtrack); - - ROS_INFO_STREAM("PlatooningTacticalPlugin Params" << config); + private: + PlatooningTacticalPluginConfig config_; - config.lateral_accel_limit = config.lateral_accel_limit * config.lat_accel_multiplier; - config.max_accel = config.max_accel * config.max_accel_multiplier; + std::shared_ptr worker_; - PlatooningTacticalPlugin worker(wm_, config, [&discovery_pub](auto msg) { discovery_pub.publish(msg); }); + public: + /** + * \brief Node constructor + */ + explicit Node(const rclcpp::NodeOptions &); - ros::ServiceServer trajectory_srv_ = nh.advertiseService("platooning_tactical_plugin/plan_trajectory", - &PlatooningTacticalPlugin::plan_trajectory_cb, &worker); + /** + * \brief Example callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + //// + // Overrides + //// + void plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; - ros::Timer discovery_pub_timer_ = nh.createTimer( - ros::Duration(ros::Rate(10.0)), - [&worker](const auto&) { worker.onSpin(); }); + bool get_availability() override; - ros::CARMANodeHandle::spin(); - } -}; + std::string get_version_id() override; + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin(); + }; } // namespace platooning_tactical_plugin \ No newline at end of file diff --git a/platooning_tactical_plugin/launch/platooning_tactical_plugin.launch b/platooning_tactical_plugin/launch/platooning_tactical_plugin.launch deleted file mode 100644 index 8d5448d167..0000000000 --- a/platooning_tactical_plugin/launch/platooning_tactical_plugin.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - diff --git a/platooning_tactical_plugin/launch/platooning_tactical_plugin_launch.py b/platooning_tactical_plugin/launch/platooning_tactical_plugin_launch.py new file mode 100644 index 0000000000..e439d9fedb --- /dev/null +++ b/platooning_tactical_plugin/launch/platooning_tactical_plugin_launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA platooning_tactical_plugin_node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') + + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='platooning_tactical_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='platooning_tactical_plugin', + plugin='platooning_tactical_plugin::Node', + name='platooning_tactical_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/platooning_tactical_plugin/package.xml b/platooning_tactical_plugin/package.xml index ab773a83c5..946cf326d5 100644 --- a/platooning_tactical_plugin/package.xml +++ b/platooning_tactical_plugin/package.xml @@ -1,42 +1,51 @@ - + -platooning_tactical_plugin -3.2.0 - -The platooning_tactical_plugin converts maneuvers from platooning strategic plugin to a CARMA trajectory - -carma -Apache License 2.0 -carma -catkin -roscpp -rospy -std_msgs -cav_srvs -cav_msgs -carma_utils -autoware_msgs -carma_wm -tf -tf2 -tf2_geometry_msgs -trajectory_utils -basic_autonomy -carma_cmake_common - \ No newline at end of file + platooning_tactical_plugin + 4.0.0 + The platooning_tactical_plugin package + + carma + + Apache 2.0 + + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + carma_guidance_plugins + std_msgs + carma_planning_msgs + autoware_msgs + carma_wm_ros2 + tf2 + tf2_geometry_msgs + trajectory_utils_ros2 + basic_autonomy_ros2 + + ament_lint_auto + ament_cmake_gtest + + launch + launch_ros + + + ament_cmake + + diff --git a/platooning_tactical_plugin/src/main.cpp b/platooning_tactical_plugin/src/main.cpp new file mode 100644 index 0000000000..c31761818f --- /dev/null +++ b/platooning_tactical_plugin/src/main.cpp @@ -0,0 +1,33 @@ +/* + * 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. + */ + +#include +#include "platooning_tactical_plugin/platooning_tactical_plugin_node.h" + +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/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp b/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp index 260cc88e5f..018949a71f 100644 --- a/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp +++ b/platooning_tactical_plugin/src/platooning_tactical_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2020 LEIDOS. + * Copyright (C) 2018-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 @@ -15,17 +15,17 @@ #include "platooning_tactical_plugin/platooning_tactical_plugin.h" -#include #include #include #include #include #include #include -#include -#include +#include +#include #include -#include +#include +#include #include #include #include @@ -37,38 +37,25 @@ using oss = std::ostringstream; namespace platooning_tactical_plugin { PlatooningTacticalPlugin::PlatooningTacticalPlugin(carma_wm::WorldModelConstPtr wm, PlatooningTacticalPluginConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher) - : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher) -{ - plugin_discovery_msg_.name = "platooning_tactical_plugin"; - plugin_discovery_msg_.version_id = "v1.0"; - plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = true; - plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL; - plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory"; -} + std::shared_ptr timer_factory) + : wm_(wm), config_(config), timer_factory_(timer_factory) +{} -bool PlatooningTacticalPlugin::onSpin() +bool PlatooningTacticalPlugin::plan_trajectory_cb(carma_planning_msgs::srv::PlanTrajectory::Request& req, + carma_planning_msgs::srv::PlanTrajectory::Response& resp) { - plugin_discovery_publisher_(plugin_discovery_msg_); - return true; -} - -bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, - cav_srvs::PlanTrajectoryResponse& resp) -{ - ros::WallTime start_time = ros::WallTime::now(); // Start timeing the execution time for planning so it can be logged + auto start_time = std::chrono::high_resolution_clock::now(); // Start timing the execution time for planning so it can be logged lanelet::BasicPoint2d veh_pos(req.vehicle_state.x_pos_global, req.vehicle_state.y_pos_global); double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack; // Only plan the trajectory for the initial LANE_FOLLOWING maneuver and any immediately sequential maneuvers of the same type - std::vector maneuver_plan; + std::vector maneuver_plan; for(size_t i = req.maneuver_index_to_plan; i < req.maneuver_plan.maneuvers.size(); i++) { - if(req.maneuver_plan.maneuvers[i].type == cav_msgs::Maneuver::LANE_FOLLOWING) + if(req.maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING) { - if (req.maneuver_plan.maneuvers[i].lane_following_maneuver.parameters.negotiation_type != cav_msgs::ManeuverParameters::NO_NEGOTIATION) + if (req.maneuver_plan.maneuvers[i].lane_following_maneuver.parameters.negotiation_type != carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION) { maneuver_plan.push_back(req.maneuver_plan.maneuvers[i]); resp.related_maneuvers.push_back(i); @@ -99,13 +86,13 @@ bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryReques auto points_and_target_speeds = basic_autonomy::waypoint_generation::create_geometry_profile(maneuver_plan, std::max((double)0, current_downtrack - config_.back_distance), wm_, ending_state_before_buffer_, req.vehicle_state, wpg_general_config, wpg_detail_config); - ROS_DEBUG_STREAM("points_and_target_speeds: " << points_and_target_speeds.size()); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_tactical_plugin"), "points_and_target_speeds: " << points_and_target_speeds.size()); - ROS_DEBUG_STREAM("PlanTrajectory"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_tactical_plugin"), "PlanTrajectory"); - cav_msgs::TrajectoryPlan original_trajectory; + carma_planning_msgs::msg::TrajectoryPlan original_trajectory; original_trajectory.header.frame_id = "map"; - original_trajectory.header.stamp = ros::Time::now(); + original_trajectory.header.stamp = timer_factory_->now(); original_trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()()); original_trajectory.trajectory_points = basic_autonomy:: waypoint_generation::compose_lanefollow_trajectory_from_path(points_and_target_speeds, req.vehicle_state, req.header.stamp, wm_, ending_state_before_buffer_, debug_msg_, @@ -119,15 +106,20 @@ bool PlatooningTacticalPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryReques debug_publisher_(debug_msg_); } - resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); + resp.maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS); - ros::WallTime end_time = ros::WallTime::now(); // Planning complete + auto end_time = std::chrono::high_resolution_clock::now(); // Planning complete - ros::WallDuration duration = end_time - start_time; - ROS_DEBUG_STREAM("ExecutionTime: " << duration.toSec()); + auto nano_s = std::chrono::duration_cast(end_time - start_time); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_tactical_plugin"), "ExecutionTime: " << ((double)nano_s.count() * 1e9)); return true; } +void PlatooningTacticalPlugin::set_config(PlatooningTacticalPluginConfig config) +{ + config_ = config; +} + } // namespace platooning_tactical_plugin diff --git a/platooning_tactical_plugin/src/platooning_tactical_plugin_node.cpp b/platooning_tactical_plugin/src/platooning_tactical_plugin_node.cpp index d4e678eed2..abdb92e3e4 100644 --- a/platooning_tactical_plugin/src/platooning_tactical_plugin_node.cpp +++ b/platooning_tactical_plugin/src/platooning_tactical_plugin_node.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * 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 @@ -13,18 +13,138 @@ * License for the specific language governing permissions and limitations under * the License. */ - -#include - -#include #include "platooning_tactical_plugin/platooning_tactical_plugin_node.h" +#include -int main(int argc, char** argv) +namespace platooning_tactical_plugin { + namespace std_ph = std::placeholders; + + Node::Node(const rclcpp::NodeOptions &options) + : carma_guidance_plugins::TacticalPlugin(options) + { + // Create initial config + config_ = PlatooningTacticalPluginConfig(); + + // Declare parameters + config_.trajectory_time_length = declare_parameter("trajectory_time_length", config_.trajectory_time_length); + config_.curve_resample_step_size = declare_parameter("curve_resample_step_size", config_.curve_resample_step_size); + config_.default_downsample_ratio = declare_parameter("default_downsample_ratio", config_.default_downsample_ratio); + config_.turn_downsample_ratio = declare_parameter("turn_downsample_ratio", config_.turn_downsample_ratio); + config_.minimum_speed = declare_parameter("minimum_speed", config_.minimum_speed); + config_.max_accel_multiplier = declare_parameter("max_accel_multiplier", config_.max_accel_multiplier); + config_.lat_accel_multiplier = declare_parameter("lat_accel_multiplier", config_.lat_accel_multiplier); + config_.back_distance = declare_parameter("back_distance", config_.back_distance); + config_.speed_moving_average_window_size = declare_parameter("speed_moving_average_window_size", config_.speed_moving_average_window_size); + config_.curvature_moving_average_window_size = declare_parameter("curvature_moving_average_window_size", config_.curvature_moving_average_window_size); + config_.max_accel = declare_parameter("vehicle_acceleration_limit", config_.max_accel); + config_.lateral_accel_limit = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); + config_.enable_object_avoidance = declare_parameter("enable_object_avoidance", config_.enable_object_avoidance); + config_.buffer_ending_downtrack = declare_parameter("buffer_ending_downtrack", config_.buffer_ending_downtrack); + + config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; + config_.max_accel = config_.max_accel * config_.max_accel_multiplier; + + RCLCPP_INFO_STREAM(get_logger(), "PlatooningTacticalPlugin Params" << config_); + + } + + rcl_interfaces::msg::SetParametersResult Node::parameter_update_callback(const std::vector ¶meters) + { + auto error = update_params({ + {"enable_object_avoidance", config_.enable_object_avoidance} + }, parameters); + + auto error2 = update_params({ + {"default_downsample_ratio", config_.default_downsample_ratio}, + {"turn_downsample_ratio", config_.turn_downsample_ratio}, + {"speed_moving_average_window_size", config_.speed_moving_average_window_size}, + {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size} + }, parameters); + + auto error3 = update_params({ + {"trajectory_time_length", config_.trajectory_time_length}, + {"curve_resample_step_size", config_.curve_resample_step_size}, + {"minimum_speed", config_.minimum_speed}, + {"max_accel_multiplier", config_.max_accel_multiplier}, + {"lat_accel_multiplier", config_.lat_accel_multiplier}, + {"back_distance", config_.back_distance}, + {"buffer_ending_downtrack", config_.buffer_ending_downtrack} + }, parameters); // Accel limits system wide and not allowed to be updated per node + + config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; + config_.max_accel = config_.max_accel * config_.max_accel_multiplier; + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error && !error2 && !error3; + + if (result.successful && worker_) + { + worker_->set_config(config_); + } + + return result; + } + + carma_ros2_utils::CallbackReturn Node::on_configure_plugin() + { + // Reset config + config_ = PlatooningTacticalPluginConfig(); + + // Load parameters + get_parameter("trajectory_time_length", config_.trajectory_time_length); + get_parameter("curve_resample_step_size", config_.curve_resample_step_size); + get_parameter("default_downsample_ratio", config_.default_downsample_ratio); + get_parameter("turn_downsample_ratio", config_.turn_downsample_ratio); + get_parameter("minimum_speed", config_.minimum_speed); + get_parameter("max_accel_multiplier", config_.max_accel_multiplier); + get_parameter("lat_accel_multiplier", config_.lat_accel_multiplier); + get_parameter("back_distance", config_.back_distance); + get_parameter("speed_moving_average_window_size", config_.speed_moving_average_window_size); + get_parameter("curvature_moving_average_window_size", config_.curvature_moving_average_window_size); + get_parameter("vehicle_acceleration_limit", config_.max_accel); + get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit); + get_parameter("enable_object_avoidance", config_.enable_object_avoidance); + get_parameter("buffer_ending_downtrack", config_.buffer_ending_downtrack); + + config_.lateral_accel_limit = config_.lateral_accel_limit * config_.lat_accel_multiplier; + config_.max_accel = config_.max_accel * config_.max_accel_multiplier; + + RCLCPP_INFO_STREAM(get_logger(), "PlatooningTacticalPlugin Params" << config_); + + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1)); + + worker_ = std::make_shared(get_world_model(), config_, + std::make_shared(shared_from_this())); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; + } + + void Node::plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) + { + if (!worker_) + return; + + worker_->plan_trajectory_cb(*req, *resp); + } + + bool Node::get_availability() { + return true; + } + + std::string Node::get_version_id() { + return "v4.0"; + } + +} // platooning_tactical_plugin - ros::init(argc, argv, "platooning_tactical_plugin"); - platooning_tactical_plugin::PlatooningTacticalPluginNode platooning_tactical_plugin; - platooning_tactical_plugin.run(); +#include "rclcpp_components/register_node_macro.hpp" - return 0; -} +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(platooning_tactical_plugin::Node) diff --git a/platooning_tactical_plugin/test/node_test.cpp b/platooning_tactical_plugin/test/node_test.cpp new file mode 100644 index 0000000000..346e2e656a --- /dev/null +++ b/platooning_tactical_plugin/test/node_test.cpp @@ -0,0 +1,37 @@ +/* + * 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. + */ + +#include +#include +#include +#include +#include +#include + +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/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp b/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp index 5fd989c049..e39b69b52c 100644 --- a/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp +++ b/platooning_tactical_plugin/test/test_platooning_tactical_plugin_planning.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -15,9 +15,8 @@ */ #include -#include +#include #include -#include #include #include #include @@ -29,12 +28,11 @@ #include #include #include -#include -#include -#include +#include +#include #include -#include -#include +#include +#include typedef Eigen::Spline Spline2d; @@ -47,7 +45,8 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallback) PlatooningTacticalPluginConfig config; config.default_downsample_ratio = 1; std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) { ROS_DEBUG_STREAM(msg); }); + PlatooningTacticalPlugin plugin(wm, config, + std::make_shared()); auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); @@ -71,40 +70,40 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallback) carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - cav_srvs::PlanTrajectoryRequest req; + carma_planning_msgs::srv::PlanTrajectory::Request req; req.vehicle_state.x_pos_global = 1.5; req.vehicle_state.y_pos_global = 5; req.vehicle_state.orientation = 0; req.vehicle_state.longitudinal_vel = 0.0; - cav_msgs::Maneuver maneuver; - maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING; - maneuver.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::GENERAL_NEGOTIATION; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::GENERAL_NEGOTIATION; maneuver.lane_following_maneuver.lane_ids = {"1200"}; maneuver.lane_following_maneuver.start_dist = 5.0; - maneuver.lane_following_maneuver.start_time = ros::Time(0.0); + maneuver.lane_following_maneuver.start_time = rclcpp::Time(0.0*1e9); maneuver.lane_following_maneuver.start_speed = 0.0; maneuver.lane_following_maneuver.end_dist = 14.98835712; maneuver.lane_following_maneuver.end_speed = 6.7056; - maneuver.lane_following_maneuver.end_time = ros::Time(4.4704); + maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704*1e9); - cav_msgs::Maneuver maneuver2; - maneuver2.type = cav_msgs::Maneuver::LANE_FOLLOWING; - maneuver2.lane_following_maneuver.parameters.negotiation_type = cav_msgs::ManeuverParameters::PLATOONING; + carma_planning_msgs::msg::Maneuver maneuver2; + maneuver2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver2.lane_following_maneuver.parameters.negotiation_type = carma_planning_msgs::msg::ManeuverParameters::PLATOONING; maneuver2.lane_following_maneuver.lane_ids = {"1200"}; maneuver2.lane_following_maneuver.start_dist = 14.98835712; maneuver2.lane_following_maneuver.start_speed = 6.7056; - maneuver2.lane_following_maneuver.start_time = ros::Time(4.4704); + maneuver2.lane_following_maneuver.start_time = rclcpp::Time(4.4704*1e9); maneuver2.lane_following_maneuver.end_dist = 14.98835712 + 50.0; maneuver2.lane_following_maneuver.end_speed = 6.7056; - maneuver2.lane_following_maneuver.end_time = ros::Time(4.4704 + 7.45645430685); + maneuver2.lane_following_maneuver.end_time = rclcpp::Time((4.4704 + 7.45645430685) * 1e9); req.maneuver_plan.maneuvers.push_back(maneuver); req.maneuver_plan.maneuvers.push_back(maneuver2); - cav_srvs::PlanTrajectoryResponse resp; + carma_planning_msgs::srv::PlanTrajectory::Response resp; plugin.plan_trajectory_cb(req, resp); @@ -116,7 +115,8 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallbackexception) PlatooningTacticalPluginConfig config; config.default_downsample_ratio = 1; std::shared_ptr wm = std::make_shared(); - PlatooningTacticalPlugin plugin(wm, config, [&](auto msg) { ROS_DEBUG_STREAM(msg); }); + PlatooningTacticalPlugin plugin(wm, config, + std::make_shared()); auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); @@ -140,38 +140,38 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallbackexception) carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - cav_srvs::PlanTrajectoryRequest req; + carma_planning_msgs::srv::PlanTrajectory::Request req; req.vehicle_state.x_pos_global = 1.5; req.vehicle_state.y_pos_global = 5; req.vehicle_state.orientation = 0; req.vehicle_state.longitudinal_vel = 0.0; - cav_msgs::Maneuver maneuver; - maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver.lane_following_maneuver.lane_ids = {"1200"}; maneuver.lane_following_maneuver.start_dist = 5.0; - maneuver.lane_following_maneuver.start_time = ros::Time(0.0); + maneuver.lane_following_maneuver.start_time = rclcpp::Time(0.0*1e9); maneuver.lane_following_maneuver.start_speed = 6.7056; maneuver.lane_following_maneuver.end_dist = 14.98835712; maneuver.lane_following_maneuver.end_speed = 6.7056; - maneuver.lane_following_maneuver.end_time = ros::Time(4.4704); + maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704*1e9); - cav_msgs::Maneuver maneuver2; - maneuver2.type = cav_msgs::Maneuver::LANE_FOLLOWING; + carma_planning_msgs::msg::Maneuver maneuver2; + maneuver2.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; maneuver2.lane_following_maneuver.lane_ids = {"1200"}; maneuver2.lane_following_maneuver.start_dist = 14.98835712; maneuver2.lane_following_maneuver.start_speed = 6.7056; - maneuver2.lane_following_maneuver.start_time = ros::Time(4.4704); + maneuver2.lane_following_maneuver.start_time = rclcpp::Time(4.4704*1e9); maneuver2.lane_following_maneuver.end_dist = 14.98835712 + 50.0; maneuver2.lane_following_maneuver.end_speed = 6.7056; - maneuver2.lane_following_maneuver.end_time = ros::Time(4.4704 + 7.45645430685); + maneuver2.lane_following_maneuver.end_time = rclcpp::Time((4.4704 + 7.45645430685)*1e9); req.maneuver_plan.maneuvers.push_back(maneuver); req.maneuver_plan.maneuvers.push_back(maneuver2); - cav_srvs::PlanTrajectoryResponse resp; + carma_planning_msgs::srv::PlanTrajectory::Response resp; // plugin.plan_trajectory_cb(req, resp); @@ -181,15 +181,3 @@ TEST(PlatooningTacticalPluginTest, testPlanningCallbackexception) } // namespace platooning_tactical_plugin - -// Run all the tests -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::Time::init(); - ROSCONSOLE_AUTOINIT; - if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { - ros::console::notifyLoggerLevelsChanged(); - } - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 842e394c8a..7cb5687b16 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -53,10 +53,11 @@ - /guidance/plugins/stop_controlled_intersection_tactical_plugin - /guidance/plugins/platoon_control_ihp - /guidance/plugins/platoon_strategic_ihp - - /guidance/plugins/platooning_tactical_plugin + - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/yield_plugin # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin + - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/cooperative_lanechange From 79a3d93ba9766f2065d5b868920d4bebb06b878f Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Wed, 24 Aug 2022 09:08:37 -0700 Subject: [PATCH 086/165] Fix single execution for loops --- ...ontrolled_intersection_tactical_plugin.cpp | 42 ++++++++++--------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 754fe498b2..b1cb191b80 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -37,22 +37,20 @@ namespace light_controlled_intersection_tactical_plugin "Light Control Intersection Plugin asked to plan invalid maneuver index: " + std::to_string(req->maneuver_index_to_plan) + " for plan of size: " + std::to_string(req->maneuver_plan.maneuvers.size())); } + std::vector maneuver_plan; - // expecting only one maneuver for an intersection - for(size_t i = req->maneuver_index_to_plan; i < req->maneuver_plan.maneuvers.size(); i++){ - - if(req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING - && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[i], parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_) - { - maneuver_plan.push_back(req->maneuver_plan.maneuvers[i]); - resp->related_maneuvers.push_back(i); - break; - } - else - { - throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver"); - } - } + // Expecting only one maneuver for an intersection, which corresponds to the maneuver_index_to_plan value provided in the request + if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING + && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan], parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_) + { + maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]); + resp->related_maneuvers.push_back(i); + break; + } + else + { + throw std::invalid_argument("Light Control Intersection Plugin asked to plan unsupported maneuver"); + } lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global); RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Planning state x:" << req->vehicle_state.x_pos_global << " , y: " << req->vehicle_state.y_pos_global); @@ -354,8 +352,12 @@ namespace light_controlled_intersection_tactical_plugin std::unordered_set visited_lanelets; std::vector processed_maneuvers; RCLCPP_DEBUG_STREAM(logger_->get_logger(), "VehDowntrack: "<get_logger(), "Creating Lane Follow Geometry"); std::vector lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); - - break; // expected to receive only one maneuver to plan + points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + } + else + { + throw std::invalid_argument("Light Control Intersection Plugin can only create a geometry profile for one maneuver"); } //Add buffer ending to lane follow points at the end of maneuver(s) end dist From 09e8b3719834c329f30af15c0ebe86ee1d70aa7e Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Wed, 24 Aug 2022 11:18:33 -0700 Subject: [PATCH 087/165] Fix build error --- .../src/light_controlled_intersection_tactical_plugin.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index b1cb191b80..cf0f94817d 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -45,7 +45,6 @@ namespace light_controlled_intersection_tactical_plugin { maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]); resp->related_maneuvers.push_back(i); - break; } else { From 612895cecd9d195e78bd015f56a8f41efb911d6c Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Wed, 24 Aug 2022 11:21:16 -0700 Subject: [PATCH 088/165] Fix build error --- .../src/light_controlled_intersection_tactical_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index cf0f94817d..270b08ade3 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -44,7 +44,7 @@ namespace light_controlled_intersection_tactical_plugin && GET_MANEUVER_PROPERTY(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan], parameters.string_valued_meta_data.front()) == light_controlled_intersection_strategy_) { maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]); - resp->related_maneuvers.push_back(i); + resp->related_maneuvers.push_back(req->maneuver_index_to_plan); } else { From 0289bbea8437b92e105f3a362a5c71bc95866847 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Thu, 25 Aug 2022 07:05:14 -0700 Subject: [PATCH 089/165] Fix logic error --- .../src/light_controlled_intersection_tactical_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index 270b08ade3..f88deda1d0 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -355,7 +355,7 @@ namespace light_controlled_intersection_tactical_plugin // Only one maneuver is expected in the received maneuver plan if(maneuvers.size() == 1) { - auto maneuver == maneuvers.front(); + auto maneuver = maneuvers.front(); double starting_downtrack = GET_MANEUVER_PROPERTY(maneuver, start_dist); From 2e6b9397b7cf41dc2227da61bcd8f11cb8655144 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 25 Aug 2022 14:14:30 -0400 Subject: [PATCH 090/165] Feature/sci strategic ros2 (#1907) Description This PR includes updates to port the sci_strategic_plugin from ROS1 to ROS2. Related Issue #1906 Motivation and Context How Has This Been Tested? All unit tests pass, and local integration tests have been conducted. --- carma/launch/guidance.launch | 7 - carma/launch/plugins.launch.py | 48 +- sci_strategic_plugin/CMakeLists.txt | 115 ++--- ...egic_plugin.h => sci_strategic_plugin.hpp} | 166 ++++--- ...nfig.h => sci_strategic_plugin_config.hpp} | 26 +- .../launch/sci_strategic_plugin.launch | 20 - .../launch/sci_strategic_plugin.launch.py | 68 +++ sci_strategic_plugin/package.xml | 34 +- sci_strategic_plugin/src/main.cpp | 78 +--- .../src/sci_strategic_plugin.cpp | 439 +++++++++++------- .../test/sci_strategic_plugin.test | 21 - sci_strategic_plugin/test/test_main.cpp | 35 +- .../test/test_sci_strategic_plugin.cpp | 60 ++- .../test/test_strategic_plugin.cpp | 291 ++++++------ .../config/guidance_controller_config.yaml | 3 +- 15 files changed, 797 insertions(+), 614 deletions(-) rename sci_strategic_plugin/include/{sci_strategic_plugin.h => sci_strategic_plugin.hpp} (69%) rename sci_strategic_plugin/include/{sci_strategic_plugin_config.h => sci_strategic_plugin_config.hpp} (64%) delete mode 100644 sci_strategic_plugin/launch/sci_strategic_plugin.launch create mode 100644 sci_strategic_plugin/launch/sci_strategic_plugin.launch.py delete mode 100644 sci_strategic_plugin/test/sci_strategic_plugin.test diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 891ba4b9aa..03556d759c 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -149,13 +149,6 @@ - - - - - - - diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 20e569fd60..0069cf7702 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -51,7 +51,7 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') cooperative_lanechange_param_file = os.path.join( get_package_share_directory('cooperative_lanechange'), 'config/parameters.yaml') @@ -59,6 +59,9 @@ def generate_launch_description(): platoon_strategic_ihp_param_file = os.path.join( get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') + sci_strategic_plugin_file_path = os.path.join( + get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') + platoon_tactical_ihp_param_file = os.path.join( get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') @@ -137,13 +140,39 @@ def generate_launch_description(): ] ), ComposableNode( - package='cooperative_lanechange', - plugin='cooperative_lanechange::CooperativeLaneChangePlugin', - name='cooperative_lanechange', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('cooperative_lanechange', env_log_levels) } - ], + package='sci_strategic_plugin', + plugin='sci_strategic_plugin::SCIStrategicPlugin', + name='sci_strategic_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('sci_strategic_plugin', env_log_levels) } + ], + 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" ] ), + ("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" ] ), + ("maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/final_maneuver_plan" ] ), + ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), + ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ), + ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ], + parameters=[ + sci_strategic_plugin_file_path, + vehicle_config_param_file + ] + ), + ComposableNode( + package='cooperative_lanechange', + plugin='cooperative_lanechange::CooperativeLaneChangePlugin', + name='cooperative_lanechange', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('cooperative_lanechange', env_log_levels) } + ], remappings = [ ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ), ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ), @@ -167,7 +196,7 @@ def generate_launch_description(): ) ] ) - + platooning_plugins_container = ComposableNodeContainer( package='carma_ros2_utils', name='platooning_plugins_container', @@ -225,6 +254,7 @@ def generate_launch_description(): ], parameters=[ platoon_tactical_ihp_param_file, vehicle_config_param_file ] ), + ] ) diff --git a/sci_strategic_plugin/CMakeLists.txt b/sci_strategic_plugin/CMakeLists.txt index 86a5d58f64..307853b00b 100644 --- a/sci_strategic_plugin/CMakeLists.txt +++ b/sci_strategic_plugin/CMakeLists.txt @@ -1,38 +1,38 @@ -cmake_minimum_required(VERSION 3.0.2) +# +# 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. +# + +cmake_minimum_required(VERSION 3.5) project(sci_strategic_plugin) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages - -set(PKG_CATKIN_DEPS - roscpp - cav_srvs - cav_msgs - carma_utils - bsm_helper - carma_wm - lanelet2_core - lanelet2_traffic_rules -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Name build targets +set(node_exec sci_strategic_plugin_exec) +set(node_lib sci_strategic_plugin_lib) -find_package(catkin REQUIRED COMPONENTS - ${PKG_CATKIN_DEPS} -) ## System dependencies are found with CMake's conventions find_package(Boost REQUIRED) - -catkin_package( - CATKIN_DEPENDS ${PKG_CATKIN_DEPS} - DEPENDS Boost -) - ########### ## Build ## ########### @@ -43,56 +43,37 @@ include_directories( include ) -# Build Lib -add_library(${PROJECT_NAME} - src/sci_strategic_plugin.cpp +# Build +ament_auto_add_library(${node_lib} SHARED + src/sci_strategic_plugin.cpp ) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -# Build Node Executable -add_executable( ${PROJECT_NAME}_node - src/main.cpp +ament_auto_add_executable(${node_exec} + src/main.cpp ) -add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) +# Register component +rclcpp_components_register_nodes(${node_lib} "sci_strategic_plugin::SCIStrategicPlugin") -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -############# -## Install ## -############# - -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Install C++ -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +# Testing +if(BUILD_TESTING) -############# -## Testing ## -############# + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -catkin_add_gmock(${PROJECT_NAME}-test - test/test_main.cpp - test/test_strategic_plugin.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) + ament_add_gtest(test_sci_strategic_plugin test/test_strategic_plugin.cpp test/test_sci_strategic_plugin.cpp test/test_main.cpp) + ament_target_dependencies(test_sci_strategic_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + target_link_libraries(test_sci_strategic_plugin ${node_lib}) -target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ${catkin_LIBRARIES}) +endif() -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_sci_strategic_plugin test/sci_strategic_plugin.test test/test_sci_strategic_plugin.cpp) - target_link_libraries(test_sci_strategic_plugin ${catkin_LIBRARIES}) -endif() \ No newline at end of file +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/sci_strategic_plugin/include/sci_strategic_plugin.h b/sci_strategic_plugin/include/sci_strategic_plugin.hpp similarity index 69% rename from sci_strategic_plugin/include/sci_strategic_plugin.h rename to sci_strategic_plugin/include/sci_strategic_plugin.hpp index f3c3df9eb8..115656a92a 100644 --- a/sci_strategic_plugin/include/sci_strategic_plugin.h +++ b/sci_strategic_plugin/include/sci_strategic_plugin.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -16,62 +16,35 @@ * the License. */ -#include +#include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include #include #include #include -#include +#include #include #include - -#include "sci_strategic_plugin_config.h" +#include +#include "sci_strategic_plugin_config.hpp" namespace sci_strategic_plugin { - enum TurnDirection { - Straight, - Right, - Left - }; +enum TurnDirection { + Straight, + Right, + Left +}; - /** - * \brief Anonymous function to extract maneuver end speed which can not be optained with GET_MANEUVER_PROPERY calls due to it missing in stop and wait plugin - * \param mvr input maneuver - * \return end speed - */ -double getManeuverEndSpeed(const cav_msgs::Maneuver& mvr) - { - switch(mvr.type) { - case cav_msgs::Maneuver::LANE_FOLLOWING: - return mvr.lane_following_maneuver.end_speed; - case cav_msgs::Maneuver::LANE_CHANGE: - return mvr.lane_change_maneuver.end_speed; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT: - return mvr.intersection_transit_straight_maneuver.end_speed; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN: - return mvr.intersection_transit_left_turn_maneuver.end_speed; - case cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN: - return mvr.intersection_transit_right_turn_maneuver.end_speed; - case cav_msgs::Maneuver::STOP_AND_WAIT: - return 0; - default: - ROS_ERROR_STREAM("Requested end speed from unsupported maneuver type"); - return 0; - } - } - - -class SCIStrategicPlugin +class SCIStrategicPlugin : public carma_guidance_plugins::StrategicPlugin { public: /** @@ -79,7 +52,7 @@ class SCIStrategicPlugin */ struct VehicleState { - ros::Time stamp; // Timestamp of this state data + rclcpp::Time stamp; // Timestamp of this state data double downtrack; // The downtrack of the vehicle along the route at time stamp double speed; // The speed of the vehicle at time stamp lanelet::Id lane_id; // The current lane id of the vehicle at time stamp @@ -87,49 +60,45 @@ class SCIStrategicPlugin /** - * \brief Constructor - * - * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data - * \param config The configuration to be used for this object - */ - SCIStrategicPlugin(carma_wm::WorldModelConstPtr wm, SCIStrategicPluginConfig& config); + * \brief Default constructor for RouteFollowingPlugin class + */ + explicit SCIStrategicPlugin(const rclcpp::NodeOptions &); + /** + * \brief Method to publish mobility operation msgs at a fixed rate of 10hz if approaching intersection. + * + */ + void publishMobilityOperation(); /** * \brief Service callback for arbitrator maneuver planning * \param req Plan maneuver request - * \param[out] resp Plan maneuver response with a list of maneuver plan + * \param resp Plan maneuver response with a list of maneuver plan * \return If service call successed */ - bool planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav_srvs::PlanManeuversResponse& resp); + void plan_maneuvers_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, + carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp); - /** - * \brief Returns the current plugin discovery message reflecting system status - * - * \return cav_msgs::Plugin The plugin discovery message + /** + * \brief Callback for dynamic parameter updates */ - cav_msgs::Plugin getDiscoveryMsg() const; - - /** - * \brief Method to call at fixed rate in execution loop. Will publish plugin discovery and mobility operation msgs. - * - * \return True if the node should continue running. False otherwise - */ - bool onSpin(); - + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); /** * \brief callback function for mobility operation * * \param msg input mobility operation msg */ - void mobilityOperationCb(const cav_msgs::MobilityOperationConstPtr& msg); + void mobilityOperationCb(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg); /** * \brief callback function for current pose * \param msg input pose stamed msg */ - void currentPoseCb(const geometry_msgs::PoseStampedConstPtr& msg); + void currentPoseCb(geometry_msgs::msg::PoseStamped::UniquePtr msg); /** * \brief Compose a lane keeping maneuver message based on input params @@ -145,16 +114,16 @@ class SCIStrategicPlugin * * \return A lane keeping maneuver message which is ready to be published */ - cav_msgs::Maneuver composeLaneFollowingManeuverMessage(int case_num, double start_dist, double end_dist, double start_speed, - double target_speed, ros::Time start_time, double time_to_stop, + carma_planning_msgs::msg::Maneuver composeLaneFollowingManeuverMessage(int case_num, double start_dist, double end_dist, double start_speed, + double target_speed, rclcpp::Time start_time, double time_to_stop, std::vector lane_ids); - cav_msgs::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed, + carma_planning_msgs::msg::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed, const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id, - double stopping_accel, ros::Time start_time, ros::Time end_time) const; + double stopping_accel, rclcpp::Time start_time, rclcpp::Time end_time) const; - cav_msgs::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed, - double target_speed, ros::Time start_time, ros::Time end_time, TurnDirection turn_direction, + carma_planning_msgs::msg::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed, + double target_speed, rclcpp::Time start_time, rclcpp::Time end_time, TurnDirection turn_direction, const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id) const; /** @@ -165,7 +134,7 @@ class SCIStrategicPlugin * * \return The extracted VehicleState */ - VehicleState extractInitialState(const cav_srvs::PlanManeuversRequest& req) const; + VehicleState extractInitialState(const carma_planning_msgs::srv::PlanManeuvers::Request& req) const; /** * \brief Helper method which calls carma_wm::WorldModel::getLaneletsBetween(start_downtrack, end_downtrack, shortest_path_only, @@ -284,12 +253,12 @@ class SCIStrategicPlugin * * \return mobility operation msg for status and intent */ - cav_msgs::MobilityOperation generateMobilityOperation(); + carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation(); /** * \brief BSM callback function */ - void BSMCb(const cav_msgs::BSMConstPtr& msg); + void BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg); /** * \brief Determine the turn direction at intersection @@ -301,6 +270,14 @@ class SCIStrategicPlugin */ TurnDirection getTurnDirectionAtIntersection(std::vector lanelets_list); + ////////// OVERRIDES /////////// + carma_ros2_utils::CallbackReturn on_configure_plugin(); + carma_ros2_utils::CallbackReturn on_activate_plugin(); + + bool get_availability(); + std::string get_version_id(); + + ////////// VARIABLES /////////// // CARMA Streets Variakes @@ -328,23 +305,26 @@ class SCIStrategicPlugin bool approaching_stop_controlled_interction_ = false; - ros::Publisher mobility_operation_pub; - ros::Publisher plugin_discovery_pub; - std::string bsm_id_ = "default_bsm_id"; uint8_t bsm_msg_count_ = 0; uint16_t bsm_sec_mark_ = 0; private: + + carma_ros2_utils::SubPtr mob_operation_sub_; + carma_ros2_utils::SubPtr current_pose_sub_; + carma_ros2_utils::SubPtr bsm_sub_; + carma_ros2_utils::PubPtr mobility_operation_pub_; + + // timer to publish mobility operation message + rclcpp::TimerBase::SharedPtr mob_op_pub_timer_; + //! World Model pointer carma_wm::WorldModelConstPtr wm_; //! Config containing configurable algorithm parameters SCIStrategicPluginConfig config_; - //! Plugin discovery message - cav_msgs::Plugin plugin_discovery_msg_; - //! Cache variables for storing the current intersection state between state machine transitions boost::optional intersection_speed_; boost::optional intersection_end_downtrack_; @@ -353,6 +333,22 @@ class SCIStrategicPlugin std::string stop_controlled_intersection_strategy_ = "Carma/stop_controlled_intersection"; std::string previous_strategy_params_ = ""; - + // Unit test helper functions + carma_wm::WorldModelConstPtr get_wm() { return wm_; } + void set_wm(carma_wm::WorldModelConstPtr new_wm) { wm_ = new_wm; } + + // Unit Test Accessors + FRIEND_TEST(SCIStrategicPluginTest, findSpeedLimit); + FRIEND_TEST(SCIStrategicPluginTest, moboperationcbtest); + FRIEND_TEST(SCIStrategicPluginTest, parseStrategyParamstest); + FRIEND_TEST(SCIStrategicPluginTest, calcEstimatedStopTimetest); + FRIEND_TEST(SCIStrategicPluginTest, calc_speed_before_deceltest); + FRIEND_TEST(SCIStrategicPluginTest, determine_speed_profile_casetest); + FRIEND_TEST(SCIStrategicPluginTest, caseOneSpeedProfiletest); + FRIEND_TEST(SCIStrategicPluginTest, caseTwoSpeedProfiletest); + FRIEND_TEST(SCIStrategicPluginTest, caseThreeSpeedProfiletest); + FRIEND_TEST(SCIStrategicPluginTest, testIntersectionturndirection); + FRIEND_TEST(SCIStrategicPluginTest, DISABLED_maneuvercbtest); + FRIEND_TEST(SCIStrategicPluginTest, maneuvercbtest); }; } // namespace sci_strategic_plugin diff --git a/sci_strategic_plugin/include/sci_strategic_plugin_config.h b/sci_strategic_plugin/include/sci_strategic_plugin_config.hpp similarity index 64% rename from sci_strategic_plugin/include/sci_strategic_plugin_config.h rename to sci_strategic_plugin/include/sci_strategic_plugin_config.hpp index a6ce9ed6c8..127d5bab6a 100644 --- a/sci_strategic_plugin/include/sci_strategic_plugin_config.h +++ b/sci_strategic_plugin/include/sci_strategic_plugin_config.hpp @@ -1,6 +1,6 @@ #pragma once /* - * Copyright (C) 2021 LEIDOS. + * 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 @@ -69,5 +69,29 @@ struct SCIStrategicPluginConfig //! License plate of the vehicle. std::string vehicle_id = "default_id"; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const SCIStrategicPluginConfig &c) + { + output << "SCIStrategicPluginConfig { " << std::endl + << "vehicle_decel_limit: " << c.vehicle_decel_limit << std::endl + << "vehicle_decel_limit_multiplier: " << c.vehicle_decel_limit_multiplier << std::endl + << "vehicle_accel_limit: " << c.vehicle_accel_limit << std::endl + << "vehicle_accel_limit_multiplier: " << c.vehicle_accel_limit_multiplier << std::endl + << "stop_line_buffer: " << c.stop_line_buffer << std::endl + << "min_maneuver_planning_period: " << c.min_maneuver_planning_period << std::endl + << "delta_t: " << c.delta_t << std::endl + << "min_gap: " << c.min_gap << std::endl + << "veh_length: " << c.veh_length << std::endl + << "reaction_time: " << c.reaction_time << std::endl + << "intersection_exit_zone_length: " << c.intersection_exit_zone_length << std::endl + << "strategic_plugin_name: " << c.strategic_plugin_name << std::endl + << "lane_following_plugin_name: " << c.lane_following_plugin_name << std::endl + << "stop_and_wait_plugin_name: " << c.stop_and_wait_plugin_name << std::endl + << "intersection_transit_plugin_name: " << c.intersection_transit_plugin_name << std::endl + << "vehicle_id: " << c.vehicle_id << std::endl + << "}" << std::endl; + return output; + } }; } // namespace localizer \ No newline at end of file diff --git a/sci_strategic_plugin/launch/sci_strategic_plugin.launch b/sci_strategic_plugin/launch/sci_strategic_plugin.launch deleted file mode 100644 index c87b029654..0000000000 --- a/sci_strategic_plugin/launch/sci_strategic_plugin.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - diff --git a/sci_strategic_plugin/launch/sci_strategic_plugin.launch.py b/sci_strategic_plugin/launch/sci_strategic_plugin.launch.py new file mode 100644 index 0000000000..08bbbb82d7 --- /dev/null +++ b/sci_strategic_plugin/launch/sci_strategic_plugin.launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA sci_strategic_plugin_container. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + sci_strategic_plugin_file_path = os.path.join( + get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='sci_strategic_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='sci_strategic_plugin', + plugin='sci_strategic_plugin::SCIStrategicPlugin', + name='sci_strategic_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ + sci_strategic_plugin_file_path + ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/sci_strategic_plugin/package.xml b/sci_strategic_plugin/package.xml index b8dd86a439..add2a4f7cc 100644 --- a/sci_strategic_plugin/package.xml +++ b/sci_strategic_plugin/package.xml @@ -1,7 +1,7 @@ - - - - - - - - \ No newline at end of file diff --git a/sci_strategic_plugin/test/test_main.cpp b/sci_strategic_plugin/test/test_main.cpp index 49fba5c9c2..3d445480c9 100644 --- a/sci_strategic_plugin/test/test_main.cpp +++ b/sci_strategic_plugin/test/test_main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 LEIDOS. + * 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 @@ -14,17 +14,24 @@ * the License. */ -#include -#include +#include +#include -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::Time::init(); - ROSCONSOLE_AUTOINIT; - if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) { - ros::console::notifyLoggerLevelsChanged(); - } - auto res = RUN_ALL_TESTS(); - return res; -} \ No newline at end of file +/*! +* \brief Main entrypoint for unit tests +*/ +int main (int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + + //Initialize ROS + rclcpp::init(argc, argv); + auto ret = rcutils_logging_set_logger_level( + rclcpp::get_logger("sci_strategic_plugin").get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + + bool success = RUN_ALL_TESTS(); + + //shutdown ROS + rclcpp::shutdown(); + + return success; +} diff --git a/sci_strategic_plugin/test/test_sci_strategic_plugin.cpp b/sci_strategic_plugin/test/test_sci_strategic_plugin.cpp index aa5e8226f0..f9a6b33593 100644 --- a/sci_strategic_plugin/test/test_sci_strategic_plugin.cpp +++ b/sci_strategic_plugin/test/test_sci_strategic_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -14,28 +14,54 @@ * the License. */ -#include -#include +#include "sci_strategic_plugin.hpp" +#include #include -#include +#include #include #include +namespace sci_strategic_plugin +{ +namespace std_ph = std::placeholders; + +class DummyNode : public carma_ros2_utils::CarmaLifecycleNode +{ +public: + + explicit DummyNode(const rclcpp::NodeOptions &options) + : carma_ros2_utils::CarmaLifecycleNode(options) + {} + + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &){return CallbackReturn::SUCCESS;} +}; + + TEST(SCIStrategicPlugin, UnitTest1) { - ros::CARMANodeHandle nh; - ros::Publisher mob_op_pub = nh.advertise("incoming_mobility_operation", 5); - ros::Publisher pose_pub = nh.advertise("current_pose", 5); - std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - ros::spinOnce(); + auto nh1 = std::make_shared(rclcpp::NodeOptions()); + auto nh2 = std::make_shared(rclcpp::NodeOptions()); - EXPECT_EQ(1, mob_op_pub.getNumSubscribers()); - EXPECT_EQ(1, pose_pub.getNumSubscribers()); + nh2->configure(); + nh2->activate(); + + auto mob_op_pub = nh1->create_publisher("incoming_mobility_operation", 5); + auto pose_pub = nh1->create_publisher("current_pose", 5); + nh1->configure(); + nh2->activate(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(nh1->get_node_base_interface()); + executor.add_node(nh2->get_node_base_interface()); + + // Spin executor for 2 seconds + auto end_time = std::chrono::system_clock::now() + std::chrono::seconds(3); + while(std::chrono::system_clock::now() < end_time){ + executor.spin_once(); + } + + EXPECT_EQ(1, mob_op_pub->get_subscription_count()); + EXPECT_EQ(1, pose_pub->get_subscription_count()); } -int main (int argc, char **argv) { - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_sci_strategic_plugin"); - auto res = RUN_ALL_TESTS(); - return res; -} \ No newline at end of file +} // namespace sci_strategic_plugin + diff --git a/sci_strategic_plugin/test/test_strategic_plugin.cpp b/sci_strategic_plugin/test/test_strategic_plugin.cpp index 0eea26427e..13910c8d7b 100644 --- a/sci_strategic_plugin/test/test_strategic_plugin.cpp +++ b/sci_strategic_plugin/test/test_strategic_plugin.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 LEIDOS. + * 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 @@ -15,46 +15,30 @@ */ #include -#include -#include "sci_strategic_plugin.h" -#include "sci_strategic_plugin_config.h" -#include -#include +#include +#include "sci_strategic_plugin.hpp" +#include "sci_strategic_plugin_config.hpp" +#include +#include // Unit tests for strategic plugin helper methods -using namespace sci_strategic_plugin; - -TEST(SCIStrategicPluginTest, getDiscoveryMsg) +namespace sci_strategic_plugin { - std::shared_ptr wm; - SCIStrategicPluginConfig config; - config.strategic_plugin_name = "test name"; - SCIStrategicPlugin sci(wm, config); - - auto msg = sci.getDiscoveryMsg(); - ASSERT_TRUE(msg.name.compare("test name") == 0); - ASSERT_TRUE(msg.version_id.compare("v1.0") == 0); - ASSERT_TRUE(msg.available); - ASSERT_TRUE(msg.activated); - ASSERT_EQ(msg.type, cav_msgs::Plugin::STRATEGIC); - ASSERT_TRUE(msg.capability.compare("strategic_plan/plan_maneuvers") == 0); -} - TEST(SCIStrategicPluginTest, composeLaneFollowingManeuverMessage) { - std::shared_ptr wm; + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); - auto result = - sci.composeLaneFollowingManeuverMessage(1, 10.2, 20.4, 5, 10, ros::Time(1.2), 1.0, { 1200, 1201 }); + sci_node->composeLaneFollowingManeuverMessage(1, 10.2, 20.4, 5, 10, rclcpp::Time(1.2*1e9), 1.0, { 1200, 1201 }); - ASSERT_EQ(cav_msgs::Maneuver::LANE_FOLLOWING, result.type); - ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, result.lane_following_maneuver.parameters.negotiation_type); - ASSERT_EQ(cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN | cav_msgs::ManeuverParameters::HAS_INT_META_DATA | - cav_msgs::ManeuverParameters::HAS_FLOAT_META_DATA | cav_msgs::ManeuverParameters::HAS_STRING_META_DATA, + ASSERT_EQ(carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING, result.type); + ASSERT_EQ(carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION, result.lane_following_maneuver.parameters.negotiation_type); + ASSERT_EQ(carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA | + carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_STRING_META_DATA, result.lane_following_maneuver.parameters.presence_vector); ASSERT_TRUE(config.lane_following_plugin_name.compare( result.lane_following_maneuver.parameters.planning_tactical_plugin) == 0); @@ -65,8 +49,8 @@ TEST(SCIStrategicPluginTest, composeLaneFollowingManeuverMessage) ASSERT_EQ(20.4, result.lane_following_maneuver.end_dist); ASSERT_EQ(5, result.lane_following_maneuver.start_speed); ASSERT_EQ(10, result.lane_following_maneuver.end_speed); - ASSERT_EQ(ros::Time(1.2), result.lane_following_maneuver.start_time); - ASSERT_EQ(ros::Time(1.2) + ros::Duration(1.0), result.lane_following_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME), result.lane_following_maneuver.start_time); + ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME) + rclcpp::Duration(1.0*1e9), result.lane_following_maneuver.end_time); ASSERT_EQ(2, result.lane_following_maneuver.lane_ids.size()); ASSERT_TRUE(result.lane_following_maneuver.lane_ids[0].compare("1200") == 0); ASSERT_TRUE(result.lane_following_maneuver.lane_ids[1].compare("1201") == 0); @@ -75,17 +59,18 @@ TEST(SCIStrategicPluginTest, composeLaneFollowingManeuverMessage) TEST(SCIStrategicPluginTest, composeIntersectionTransitMessage) { - std::shared_ptr wm; + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); TurnDirection intersection_turn_direction = TurnDirection::Straight; - auto result = sci.composeIntersectionTransitMessage(10.2, 20.4, 5, 10, ros::Time(1.2), ros::Time(2.2), intersection_turn_direction, 1200, 1201); + auto result = sci_node->composeIntersectionTransitMessage(10.2, 20.4, 5, 10, rclcpp::Time(1.2*1e9), rclcpp::Time(2.2*1e9), intersection_turn_direction, 1200, 1201); - ASSERT_EQ(cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT, result.type); - ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, + ASSERT_EQ(carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT, result.type); + ASSERT_EQ(carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION, result.intersection_transit_straight_maneuver.parameters.negotiation_type); - ASSERT_EQ(cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN, + ASSERT_EQ(carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN, result.intersection_transit_straight_maneuver.parameters.presence_vector); ASSERT_TRUE(config.intersection_transit_plugin_name.compare( result.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin) == 0); @@ -96,23 +81,23 @@ TEST(SCIStrategicPluginTest, composeIntersectionTransitMessage) ASSERT_EQ(20.4, result.intersection_transit_straight_maneuver.end_dist); ASSERT_EQ(5, result.intersection_transit_straight_maneuver.start_speed); ASSERT_EQ(10, result.intersection_transit_straight_maneuver.end_speed); - ASSERT_EQ(ros::Time(1.2), result.intersection_transit_straight_maneuver.start_time); - ASSERT_EQ(ros::Time(2.2), result.intersection_transit_straight_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME), result.intersection_transit_straight_maneuver.start_time); + ASSERT_EQ(rclcpp::Time(2.2*1e9, RCL_ROS_TIME), result.intersection_transit_straight_maneuver.end_time); ASSERT_TRUE(result.intersection_transit_straight_maneuver.starting_lane_id.compare("1200") == 0); ASSERT_TRUE(result.intersection_transit_straight_maneuver.ending_lane_id.compare("1201") == 0); } TEST(SCIStrategicPluginTest, composeStopAndWaitManeuverMessage) { - std::shared_ptr wm; + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); - - auto result = sci.composeStopAndWaitManeuverMessage(10.2, 20.4, 5, 1200, 1201, 0.56, ros::Time(1.2), ros::Time(2.2)); + auto result = sci_node->composeStopAndWaitManeuverMessage(10.2, 20.4, 5, 1200, 1201, 0.56, rclcpp::Time(1.2*1e9), rclcpp::Time(2.2*1e9)); - ASSERT_EQ(cav_msgs::Maneuver::STOP_AND_WAIT, result.type); - ASSERT_EQ(cav_msgs::ManeuverParameters::NO_NEGOTIATION, result.stop_and_wait_maneuver.parameters.negotiation_type); - ASSERT_EQ(cav_msgs::ManeuverParameters::HAS_TACTICAL_PLUGIN | cav_msgs::ManeuverParameters::HAS_FLOAT_META_DATA, + ASSERT_EQ(carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT, result.type); + ASSERT_EQ(carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION, result.stop_and_wait_maneuver.parameters.negotiation_type); + ASSERT_EQ(carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA, result.stop_and_wait_maneuver.parameters.presence_vector); ASSERT_TRUE(config.stop_and_wait_plugin_name.compare( result.stop_and_wait_maneuver.parameters.planning_tactical_plugin) == 0); @@ -122,17 +107,19 @@ TEST(SCIStrategicPluginTest, composeStopAndWaitManeuverMessage) ASSERT_EQ(10.2, result.stop_and_wait_maneuver.start_dist); ASSERT_EQ(20.4, result.stop_and_wait_maneuver.end_dist); ASSERT_EQ(5, result.stop_and_wait_maneuver.start_speed); - ASSERT_EQ(ros::Time(1.2), result.stop_and_wait_maneuver.start_time); - ASSERT_EQ(ros::Time(2.2), result.stop_and_wait_maneuver.end_time); + ASSERT_EQ(rclcpp::Time(1.2*1e9, RCL_ROS_TIME), result.stop_and_wait_maneuver.start_time); + ASSERT_EQ(rclcpp::Time(2.2*1e9, RCL_ROS_TIME), result.stop_and_wait_maneuver.end_time); ASSERT_EQ(0.56, result.stop_and_wait_maneuver.parameters.float_valued_meta_data[1]); ASSERT_TRUE(result.stop_and_wait_maneuver.starting_lane_id.compare("1200") == 0); ASSERT_TRUE(result.stop_and_wait_maneuver.ending_lane_id.compare("1201") == 0); } - - TEST(SCIStrategicPluginTest, findSpeedLimit) { + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + std::shared_ptr wm; carma_wm::test::MapOptions options; options.lane_length_ = 25; @@ -142,53 +129,56 @@ TEST(SCIStrategicPluginTest, findSpeedLimit) wm = carma_wm::test::getGuidanceTestMap(options); carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + sci_node->set_wm(wm); auto ll_iterator = wm->getMap()->laneletLayer.find(1200); if (ll_iterator == wm->getMap()->laneletLayer.end()) FAIL() << "Expected lanelet not present in map. Unit test may not be structured correctly"; - ASSERT_NEAR(11.176, sci.findSpeedLimit(*ll_iterator), 0.00001); + ASSERT_NEAR(11.176, sci_node->findSpeedLimit(*ll_iterator), 0.00001); } TEST(SCIStrategicPluginTest, moboperationcbtest) { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg.strategy = "Carma/stop_controlled_intersection"; std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); + ASSERT_EQ(sci_node->approaching_stop_controlled_interction_, false); + auto msg_ptr = std::make_unique(msg); + sci_node->mobilityOperationCb(std::move(msg_ptr)); - ASSERT_EQ(sci.approaching_stop_controlled_interction_, false); - auto msg_ptr = boost::make_shared(msg); - sci.mobilityOperationCb(msg_ptr); - - ASSERT_EQ(sci.approaching_stop_controlled_interction_, true); + ASSERT_EQ(sci_node->approaching_stop_controlled_interction_, true); } TEST(SCIStrategicPluginTest, parseStrategyParamstest) { - cav_msgs::MobilityOperation msg; + carma_v2x_msgs::msg::MobilityOperation msg; msg.strategy_params = "st:16000,et:32000,dt:48000,dp:1,access:0"; std::shared_ptr wm = std::make_shared(); SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); - sci.parseStrategyParams(msg.strategy_params); + sci_node->parseStrategyParams(msg.strategy_params); - EXPECT_EQ(16000, sci.scheduled_stop_time_); - EXPECT_EQ(32000, sci.scheduled_enter_time_); - EXPECT_EQ(48000, sci.scheduled_depart_time_); - EXPECT_EQ(1, sci.scheduled_departure_position_); - EXPECT_EQ(false, sci.is_allowed_int_); + EXPECT_EQ(16000, sci_node->scheduled_stop_time_); + EXPECT_EQ(32000, sci_node->scheduled_enter_time_); + EXPECT_EQ(48000, sci_node->scheduled_depart_time_); + EXPECT_EQ(1, sci_node->scheduled_departure_position_); + EXPECT_EQ(false, sci_node->is_allowed_int_); - cav_msgs::MobilityOperation outgoing_msg = sci.generateMobilityOperation(); + carma_v2x_msgs::msg::MobilityOperation outgoing_msg = sci_node->generateMobilityOperation(); EXPECT_EQ(outgoing_msg.strategy, "Carma/stop_controlled_intersection"); EXPECT_EQ(outgoing_msg.m_header.sender_id, config.vehicle_id); std::cout << "strategy_param: " << outgoing_msg.strategy_params << std::endl; @@ -197,10 +187,12 @@ TEST(SCIStrategicPluginTest, parseStrategyParamstest) TEST(SCIStrategicPluginTest, calcEstimatedStopTimetest) { std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); - double stop_time = sci.calcEstimatedStopTime(25, 13); + double stop_time = sci_node->calcEstimatedStopTime(25, 13); EXPECT_NEAR(3.84, stop_time, 0.01); } @@ -209,10 +201,12 @@ TEST(SCIStrategicPluginTest, calc_speed_before_deceltest) { std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); - double stop_speed = sci.calc_speed_before_decel(20, 250, 10); + double stop_speed = sci_node->calc_speed_before_decel(20, 250, 10); EXPECT_NEAR(21.5, stop_speed, 0.2); } @@ -220,18 +214,20 @@ TEST(SCIStrategicPluginTest, calc_speed_before_deceltest) TEST(SCIStrategicPluginTest, determine_speed_profile_casetest) { std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); - int case_num1 = sci.determine_speed_profile_case(50, 15, 40, 10); + int case_num1 = sci_node->determine_speed_profile_case(50, 15, 40, 10); EXPECT_EQ(3, case_num1); - int case_num2 = sci.determine_speed_profile_case(100, 13, 11, 10); + int case_num2 = sci_node->determine_speed_profile_case(100, 13, 11, 10); EXPECT_EQ(2, case_num2); - int case_num3 = sci.determine_speed_profile_case(100, 13, 11, 20); + int case_num3 = sci_node->determine_speed_profile_case(100, 13, 11, 20); EXPECT_EQ(1, case_num3); @@ -240,12 +236,14 @@ TEST(SCIStrategicPluginTest, determine_speed_profile_casetest) TEST(SCIStrategicPluginTest, caseOneSpeedProfiletest) { std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); std::vector metadata{}; - sci.caseOneSpeedProfile(17, 12, 44, &metadata); + sci_node->caseOneSpeedProfile(17, 12, 44, &metadata); EXPECT_NEAR(0.5, metadata[0], 0.01); EXPECT_NEAR(-0.5, metadata[1], 0.01); @@ -256,12 +254,14 @@ TEST(SCIStrategicPluginTest, caseOneSpeedProfiletest) TEST(SCIStrategicPluginTest, caseTwoSpeedProfiletest) { std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); std::vector metadata{}; - sci.caseTwoSpeedProfile(250, 21.2, 10, 20, 15, &metadata); + sci_node->caseTwoSpeedProfile(250, 21.2, 10, 20, 15, &metadata); EXPECT_NEAR(1, metadata[0], 0.01); EXPECT_NEAR(1, metadata[1], 0.01); @@ -273,10 +273,12 @@ TEST(SCIStrategicPluginTest, caseTwoSpeedProfiletest) TEST(SCIStrategicPluginTest, caseThreeSpeedProfiletest) { std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); - double dec_val = sci.caseThreeSpeedProfile(50, 5, 30); + double dec_val = sci_node->caseThreeSpeedProfile(50, 5, 30); EXPECT_NEAR(-1.83, dec_val, 0.01); } @@ -284,10 +286,12 @@ TEST(SCIStrategicPluginTest, caseThreeSpeedProfiletest) TEST(SCIStrategicPluginTest, testIntersectionturndirection) { std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(wm, config); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(wm); - double dec_val = sci.caseThreeSpeedProfile(50, 5, 30); + double dec_val = sci_node->caseThreeSpeedProfile(50, 5, 30); EXPECT_NEAR(-1.83, dec_val, 0.01); } @@ -295,7 +299,7 @@ TEST(SCIStrategicPluginTest, testIntersectionturndirection) // The map in this unit test does not support turn direction and therefore it is disabled. // The test can be run if the turn direction detection logic (lines 461-467) is commented. -TEST(SCIStrategicPluginTest, DISABLE_maneuvercbtest) +TEST(SCIStrategicPluginTest, DISABLED_maneuvercbtest) { lanelet::Id id{1200}; // intersection id @@ -331,73 +335,76 @@ TEST(SCIStrategicPluginTest, DISABLE_maneuvercbtest) // Create a complete map carma_wm::test::MapOptions mp(1,1); auto cmw_ptr = carma_wm::test::getGuidanceTestMap(mp); - std::shared_ptr row = lanelet::AllWayStop::make(int_id, lanelet::AttributeMap(), {{ll1, ls1}, {ll3, ls4}}); + cmw_ptr->getMutableMap()->update(cmw_ptr->getMutableMap()->laneletLayer.get(1200), row); carma_wm::test::setRouteByIds({1200, 1201, 1202, 1203}, cmw_ptr); + auto sci_node = std::make_shared(rclcpp::NodeOptions()); + sci_node->configure(); + sci_node->activate(); + sci_node->set_wm(cmw_ptr); - std::shared_ptr wm = std::make_shared(); - SCIStrategicPluginConfig config; - SCIStrategicPlugin sci(cmw_ptr, config); - - sci.current_downtrack_ = 1.0; + sci_node->current_downtrack_ = 1.0; // pose callback test - geometry_msgs::PoseStamped pose_msg; + geometry_msgs::msg::PoseStamped pose_msg; pose_msg.pose.position.x = 1.0; pose_msg.pose.position.y = 1.0; - auto msg = boost::make_shared(pose_msg); - sci.currentPoseCb(msg); - ASSERT_NEAR(1.0, sci.current_downtrack_, 0.1); + auto msg = std::make_unique(pose_msg); + sci_node->currentPoseCb(std::move(msg)); + ASSERT_NEAR(1.0, sci_node->current_downtrack_, 0.1); + sci_node->approaching_stop_controlled_interction_ = true; + sci_node->street_msg_timestamp_ = 2000; + sci_node->scheduled_stop_time_ = 2500; + sci_node->scheduled_enter_time_ = 5000; + sci_node->scheduled_depart_time_ = 7000; - sci.approaching_stop_controlled_interction_ = true; - sci.street_msg_timestamp_ = 2000; - sci.scheduled_stop_time_ = 2500; - sci.scheduled_enter_time_ = 5000; - sci.scheduled_depart_time_ = 7000; - - - cav_srvs::PlanManeuversRequest req; - cav_srvs::PlanManeuversResponse resp; + auto srv_header = std::make_shared(); + auto req = std::make_shared(); + auto resp = std::make_shared(); // approaching intersection - req = cav_srvs::PlanManeuversRequest(); - req.veh_x = 1.85; - req.veh_y = 1.0; - req.veh_downtrack = req.veh_y; - req.veh_logitudinal_velocity = 11.176; - req.veh_lane_id = "1200"; + req->veh_x = 1.85; + req->veh_y = 1.0; + req->veh_downtrack = req->veh_y; + req->veh_logitudinal_velocity = 11.176; + req->veh_lane_id = "1200"; - + sci_node->plan_maneuvers_callback(srv_header, req, resp); + + ASSERT_EQ(1, resp->new_plan.maneuvers.size()); - sci.planManeuverCb(req, resp); + ASSERT_EQ(resp->new_plan.maneuvers[0].lane_following_maneuver.lane_ids[0], "1200"); + + ASSERT_NEAR(0.0, resp->new_plan.maneuvers[0].lane_following_maneuver.end_speed, 0.00001); - ASSERT_EQ(1, resp.new_plan.maneuvers.size()); - ASSERT_EQ(resp.new_plan.maneuvers[0].lane_following_maneuver.lane_ids[0], "1200"); - ASSERT_NEAR(0.0, resp.new_plan.maneuvers[0].lane_following_maneuver.end_speed, 0.00001); // case 3 - ASSERT_EQ(2, resp.new_plan.maneuvers[0].lane_following_maneuver.parameters.int_valued_meta_data[0]); + ASSERT_EQ(2, resp->new_plan.maneuvers[0].lane_following_maneuver.parameters.int_valued_meta_data[0]); - // at the stop line - cav_srvs::PlanManeuversRequest req1; - cav_srvs::PlanManeuversResponse resp1; - sci.current_downtrack_ = 9; - req1 = cav_srvs::PlanManeuversRequest(); - req1.veh_x = 9.85; - req1.veh_y = 2.0; - req1.veh_downtrack = req.veh_y; - req1.veh_logitudinal_velocity = 0.0; - req1.veh_lane_id = "1209"; + // at the stop line + auto srv_header1 = std::make_shared(); + auto req1 = std::make_shared(); + auto resp1 = std::make_shared(); - sci.scheduled_enter_time_ = 7000; + sci_node->current_downtrack_ = 9; + req1->veh_x = 9.85; + req1->veh_y = 2.0; + req1->veh_downtrack = req->veh_y; + req1->veh_logitudinal_velocity = 0.0; + req1->veh_lane_id = "1209"; - sci.planManeuverCb(req1, resp1); - ASSERT_EQ(1, resp1.new_plan.maneuvers.size()); - ASSERT_EQ(resp1.new_plan.maneuvers[0].stop_and_wait_maneuver.starting_lane_id, "1212"); - ASSERT_EQ(resp1.new_plan.maneuvers[0].stop_and_wait_maneuver.ending_lane_id, "1212"); + sci_node->scheduled_enter_time_ = 7000; + sci_node->plan_maneuvers_callback(srv_header1, req1, resp1); + ASSERT_EQ(1, resp1->new_plan.maneuvers.size()); + ASSERT_EQ(resp1->new_plan.maneuvers[0].stop_and_wait_maneuver.starting_lane_id, "1212"); + ASSERT_EQ(resp1->new_plan.maneuvers[0].stop_and_wait_maneuver.ending_lane_id, "1212"); } + + + +} // namespace sci_strategic_plugin \ No newline at end of file diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 7cb5687b16..435eb122c8 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -59,5 +59,6 @@ # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin - - /guidance/plugins/platooning_tactical_plugin_node + - /guidance/plugins/sci_strategic_plugin - /guidance/plugins/cooperative_lanechange + - /guidance/plugins/platooning_tactical_plugin_node \ No newline at end of file From 5ab0c3f94d6d7e52d26165c3861cb5616ef6f976 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Fri, 26 Aug 2022 10:09:06 -0400 Subject: [PATCH 091/165] fix (#1913) Description This is a fix for the remapping in strategic_plugin ROS2 port happened here #1907 Related Issue #1906 --- carma/launch/plugins.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index 0069cf7702..7b36cbc3a0 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -156,7 +156,7 @@ def generate_launch_description(): ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ), ("maneuver_plan", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/final_maneuver_plan" ] ), ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), - ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ), + ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ), ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), ], From 78f6e104863d68b80141c112a7e6b04fa03aa722 Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Fri, 26 Aug 2022 08:57:36 -0700 Subject: [PATCH 092/165] Update sonar for platooning_strategic_ihp --- .sonarqube/sonar-scanner.properties | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index 4cf717811d..dfd4c53af4 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -48,7 +48,7 @@ sonar.modules= bsm_generator, \ carma_wm_ctrl, \ mpc_follower_wrapper, \ roadway_objects, \ - platooning_strategic, \ + platooning_strategic_ihp, \ mock_lightbar_driver, \ platooning_tactical_plugin, \ port_drayage_plugin, \ @@ -93,7 +93,7 @@ carma_wm_ctrl.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/ca mpc_follower_wrapper.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/mpc_follower_wrapper truck_inspection_client.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/truck_inspection_client roadway_objects.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/roadway_objects -platooning_strategic.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_strategic +platooning_strategic_ihp.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_strategic_ihp platooning_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_tactical_plugin platoon_control_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_control mobilitypath_publisher.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/mobilitypath_publisher @@ -156,8 +156,8 @@ truck_inspection_client.sonar.sources = src truck_inspection_client.sonar.exclusions =test/** roadway_objects.sonar.sources = src roadway_objects.sonar.exclusions =test/** -platooning_strategic.sonar.sources = src -platooning_strategic.sonar.exclusions =test/** +platooning_strategic_ihp.sonar.sources = src +platooning_strategic_ihp.sonar.exclusions =test/** platooning_tactical_plugin.sonar.sources = src platooning_tactical_plugin.sonar.exclusions =test/** platoon_control_plugin.sonar.sources = src @@ -229,7 +229,7 @@ carma_wm_ctrl.sonar.tests = test mpc_follower_wrapper.sonar.tests = test truck_inspection_client.sonar.tests = test roadway_objects.sonar.tests = test -platooning_strategic.sonar.tests = test +platooning_strategic_ihp.sonar.tests = test platooning_tactical_plugin.sonar.tests = test platoon_control_plugin.sonar.tests = test mobilitypath_publisher.sonar.tests = test From d65b3beb1a36052c5d1bfc311bead5c5d486086c Mon Sep 17 00:00:00 2001 From: CARMA Developer Date: Fri, 26 Aug 2022 09:46:24 -0700 Subject: [PATCH 093/165] Fix platooning_strategic_IHP name in sonar --- .sonarqube/sonar-scanner.properties | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.sonarqube/sonar-scanner.properties b/.sonarqube/sonar-scanner.properties index dfd4c53af4..1b28b03d17 100644 --- a/.sonarqube/sonar-scanner.properties +++ b/.sonarqube/sonar-scanner.properties @@ -48,7 +48,7 @@ sonar.modules= bsm_generator, \ carma_wm_ctrl, \ mpc_follower_wrapper, \ roadway_objects, \ - platooning_strategic_ihp, \ + platooning_strategic_IHP, \ mock_lightbar_driver, \ platooning_tactical_plugin, \ port_drayage_plugin, \ @@ -93,7 +93,7 @@ carma_wm_ctrl.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/ca mpc_follower_wrapper.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/mpc_follower_wrapper truck_inspection_client.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/truck_inspection_client roadway_objects.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/roadway_objects -platooning_strategic_ihp.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_strategic_ihp +platooning_strategic_IHP.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_strategic_IHP platooning_tactical_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_tactical_plugin platoon_control_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/platooning_control mobilitypath_publisher.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/mobilitypath_publisher @@ -156,8 +156,8 @@ truck_inspection_client.sonar.sources = src truck_inspection_client.sonar.exclusions =test/** roadway_objects.sonar.sources = src roadway_objects.sonar.exclusions =test/** -platooning_strategic_ihp.sonar.sources = src -platooning_strategic_ihp.sonar.exclusions =test/** +platooning_strategic_IHP.sonar.sources = src +platooning_strategic_IHP.sonar.exclusions =test/** platooning_tactical_plugin.sonar.sources = src platooning_tactical_plugin.sonar.exclusions =test/** platoon_control_plugin.sonar.sources = src @@ -229,7 +229,7 @@ carma_wm_ctrl.sonar.tests = test mpc_follower_wrapper.sonar.tests = test truck_inspection_client.sonar.tests = test roadway_objects.sonar.tests = test -platooning_strategic_ihp.sonar.tests = test +platooning_strategic_IHP.sonar.tests = test platooning_tactical_plugin.sonar.tests = test platoon_control_plugin.sonar.tests = test mobilitypath_publisher.sonar.tests = test From 8c43480b9ad0953f3724145a33bd0ffacd11a00b Mon Sep 17 00:00:00 2001 From: Aswath <32398753+aswath1@users.noreply.github.com> Date: Sat, 27 Aug 2022 01:37:28 +0530 Subject: [PATCH 094/165] yield (#1886) Description Port the yield_plugin node from ROS1 to ROS2 Related Issue #1885 Co-authored-by: Misheel Bayartsengel --- carma/launch/guidance.launch | 6 - carma/launch/guidance.launch.py | 3 + carma/launch/plugins.launch.py | 98 +++++- .../config/guidance_controller_config.yaml | 1 + yield_plugin/CMakeLists.txt | 105 +++--- .../{yield_config.h => yield_config.hpp} | 0 .../{yield_plugin.h => yield_plugin.hpp} | 103 +++--- .../include/yield_plugin/yield_plugin_node.h | 94 ------ .../yield_plugin/yield_plugin_node.hpp | 84 +++++ yield_plugin/launch/yield_plugin.launch | 20 -- yield_plugin/launch/yield_plugin.launch.py | 67 ++++ yield_plugin/package.xml | 35 +- yield_plugin/src/main.cpp | 24 +- yield_plugin/src/yield_plugin.cpp | 237 +++++++------- yield_plugin/src/yield_plugin_node.cpp | 117 +++++++ yield_plugin/test/test_cooperative_yield.cpp | 163 ++++----- yield_plugin/test/test_yield.cpp | 308 +++++++++--------- yield_plugin/test/test_yield_plugin.cpp | 176 ++++++---- yield_plugin/test/yield_plugin.test | 21 -- 19 files changed, 963 insertions(+), 699 deletions(-) rename yield_plugin/include/yield_plugin/{yield_config.h => yield_config.hpp} (100%) rename yield_plugin/include/yield_plugin/{yield_plugin.h => yield_plugin.hpp} (71%) delete mode 100644 yield_plugin/include/yield_plugin/yield_plugin_node.h create mode 100644 yield_plugin/include/yield_plugin/yield_plugin_node.hpp delete mode 100644 yield_plugin/launch/yield_plugin.launch create mode 100644 yield_plugin/launch/yield_plugin.launch.py create mode 100644 yield_plugin/src/yield_plugin_node.cpp mode change 100644 => 100755 yield_plugin/test/test_cooperative_yield.cpp mode change 100644 => 100755 yield_plugin/test/test_yield.cpp mode change 100644 => 100755 yield_plugin/test/test_yield_plugin.cpp delete mode 100644 yield_plugin/test/yield_plugin.test diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 5d8867cf17..99918ed96b 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -138,12 +138,6 @@ - - - - - - diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index 6308da2974..bd65c1d828 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -64,6 +64,9 @@ def generate_launch_description(): route_param_file = os.path.join( get_package_share_directory('route'), 'config/parameters.yaml') + trajectory_visualizer_param_file = os.path.join( + get_package_share_directory('trajectory_visualizer'), 'config/parameters.yaml') + guidance_param_file = os.path.join( get_package_share_directory('guidance'), 'config/parameters.yaml') diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index e7919986a4..a604c77c84 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -61,10 +61,13 @@ def generate_launch_description(): platoon_strategic_ihp_param_file = os.path.join( get_package_share_directory('platoon_strategic_ihp'), 'config/parameters.yaml') - + sci_strategic_plugin_file_path = os.path.join( - get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') - + get_package_share_directory('sci_strategic_plugin'), 'config/parameters.yaml') + + yield_plugin_file_path = os.path.join( + get_package_share_directory('yield_plugin'), 'config/parameters.yaml') + platoon_tactical_ihp_param_file = os.path.join( get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') @@ -197,6 +200,32 @@ def generate_launch_description(): vehicle_config_param_file ] ), + ComposableNode( + package='yield_plugin', + plugin='yield_plugin::YieldPluginNode', + name='yield_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('yield_plugin', env_log_levels) } + ], + 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" ] ), + ("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" ] ), + ("outgoing_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_response" ] ), + ("incoming_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ), + ("cooperative_lane_change_status", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/cooperative_lane_change_status" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference"]), + ("bsm_outbound", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/bsm_outbound" ] ), + ], + parameters=[ + yield_plugin_file_path, + vehicle_config_param_file + ] + ), ComposableNode( package='light_controlled_intersection_tactical_plugin', plugin='light_controlled_intersection_tactical_plugin::LightControlledIntersectionTransitPluginNode', @@ -284,6 +313,69 @@ def generate_launch_description(): + platooning_plugins_container = ComposableNodeContainer( + package='carma_ros2_utils', + name='platooning_plugins_container', + executable='carma_component_container_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='platoon_strategic_ihp', + plugin='platoon_strategic_ihp::Node', + name='platoon_strategic_ihp_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('platoon_strategic_ihp', env_log_levels) } + ], + 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" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("outgoing_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_response" ] ), + ("outgoing_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_request" ] ), + ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), + ("incoming_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ), + ("incoming_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_response" ] ), + ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), + ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ), + ("twist_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/twist_raw" ] ), + ("platoon_info", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/platoon_info" ] ), + ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ), + ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ), + ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ], + parameters=[ + platoon_strategic_ihp_param_file, + vehicle_config_param_file + ] + ), + ComposableNode( + package='platooning_tactical_plugin', + plugin='platooning_tactical_plugin::Node', + name='platooning_tactical_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('platooning_tactical_plugin', env_log_levels) } + ], + 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" ] ), + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("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" ] ), + ], + parameters=[ platoon_tactical_ihp_param_file, vehicle_config_param_file ] + ), + + ] + ) + + + return LaunchDescription([ carma_plugins_container, platooning_plugins_container diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index f32f271a9b..adee30bf6c 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -59,6 +59,7 @@ # List of guidance plugins that are ported to ROS2. If not in this list, it is assumed to be ROS1, and not managed. ros2_initial_plugins: - /guidance/plugins/inlanecruising_plugin + - /guidance/plugins/yield_plugin - /guidance/plugins/light_controlled_intersection_tactical_plugin - /guidance/plugins/platooning_tactical_plugin_node - /guidance/plugins/sci_strategic_plugin diff --git a/yield_plugin/CMakeLists.txt b/yield_plugin/CMakeLists.txt index 8b156ac3ff..49f1b77e96 100644 --- a/yield_plugin/CMakeLists.txt +++ b/yield_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2018-2020 LEIDOS. +# 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 @@ -14,95 +14,78 @@ # the License. # -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(yield_plugin) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -set( CATKIN_DEPS - roscpp - std_msgs - cav_msgs - cav_srvs - carma_utils - trajectory_utils - autoware_msgs - carma_wm - lanelet2_extension -) +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) +# Name build targets +set(node_exec yield_plugin_exec) +set(worker_lib yield_plugin_lib) +set(node_lib yield_plugin_node) ## System dependencies are found with CMake's conventions find_package(Boost) find_package(Eigen3 REQUIRED) -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost Eigen3 -) - ########### ## Build ## ########### include_directories( include - ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ) -add_executable( ${PROJECT_NAME} - src/main.cpp) -add_library(yield_plugin_library - src/yield_plugin.cpp +# Build +ament_auto_add_library(${node_lib} SHARED + src/yield_plugin.cpp + src/yield_plugin_node.cpp ) -target_link_libraries(yield_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(yield_plugin_library ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} yield_plugin_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${PROJECT_NAME} yield_plugin_library ${catkin_EXPORTED_TARGETS}) +ament_auto_add_executable(${node_exec} + src/main.cpp +) -############# -## Install ## -############# +# Register component +rclcpp_components_register_nodes(${node_lib} "yield_plugin::YieldPluginNode") -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_lib} + ${Boost_LIBRARIES} + ${EIGEN3_LIBRARIES} ) - -## Install C++ -install(TARGETS ${PROJECT_NAME} yield_plugin_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +target_link_libraries(${node_exec} + ${node_lib} ) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -############# -## Testing ## -############# + ament_add_gtest(test_yield_plugin + test/test_cooperative_yield.cpp + test/test_yield_plugin.cpp + test/test_yield.cpp + ) -catkin_add_gmock(${PROJECT_NAME}-test test/test_yield.cpp test/test_cooperative_yield.cpp) -target_link_libraries(${PROJECT_NAME}-test yield_plugin_library ${catkin_LIBRARIES}) + ament_target_dependencies(test_yield_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + + target_link_libraries(test_yield_plugin ${node_lib}) -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(test_yield_plugin test/yield_plugin.test test/test_yield_plugin.cpp) - target_link_libraries(test_yield_plugin ${catkin_LIBRARIES}) endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/yield_plugin/include/yield_plugin/yield_config.h b/yield_plugin/include/yield_plugin/yield_config.hpp similarity index 100% rename from yield_plugin/include/yield_plugin/yield_config.h rename to yield_plugin/include/yield_plugin/yield_config.hpp diff --git a/yield_plugin/include/yield_plugin/yield_plugin.h b/yield_plugin/include/yield_plugin/yield_plugin.hpp similarity index 71% rename from yield_plugin/include/yield_plugin/yield_plugin.h rename to yield_plugin/include/yield_plugin/yield_plugin.hpp index 2938a3a425..f25dc5d6c7 100644 --- a/yield_plugin/include/yield_plugin/yield_plugin.h +++ b/yield_plugin/include/yield_plugin/yield_plugin.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2019-2020 LEIDOS. + * 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 @@ -17,37 +17,41 @@ */ #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include -#include +#include #include -#include -#include -#include +#include #include -#include "yield_config.h" +#include "yield_config.hpp" #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include #include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include namespace yield_plugin { -using PublishPluginDiscoveryCB = std::function; -using MobilityResponseCB = std::function; -using LaneChangeStatusCB = std::function; +using MobilityResponseCB = std::function; +using LaneChangeStatusCB = std::function; /** * \brief Convenience class for pairing 2d points with speeds @@ -70,32 +74,23 @@ class YieldPlugin * * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data * \param config The configuration to be used for this object - * \param plugin_discovery_publisher Callback which will publish the current plugin discovery state * \param mobility_response_publisher Callback which will publish the mobility response * \param lc_status_publisher Callback which will publish the cooperative lane change status */ - YieldPlugin(carma_wm::WorldModelConstPtr wm, YieldPluginConfig config, - PublishPluginDiscoveryCB plugin_discovery_publisher, + YieldPlugin(std::shared_ptr nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config, MobilityResponseCB mobility_response_publisher, LaneChangeStatusCB lc_status_publisher); - /** - * \brief Method to call at fixed rate in execution loop. Will publish plugin discovery updates - * - * \return True if the node should continue running. False otherwise - */ - bool onSpin(); - - /** * \brief Service callback for trajectory planning - * + * \param srv_header header * \param req The service request * \param resp The service response * - * \return True if success. False otherwise */ - bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp); + void plan_trajectory_callback( + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp); /** * \brief trajectory is modified to safely avoid obstacles on the road @@ -103,7 +98,7 @@ class YieldPlugin * \param current_speed_ current speed of the vehicle * \return modified trajectory plan */ - cav_msgs::TrajectoryPlan update_traj_for_object(const cav_msgs::TrajectoryPlan& original_tp, double initial_velocity); + carma_planning_msgs::msg::TrajectoryPlan update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_velocity); /** * \brief calculate quintic polynomial equation for a given x @@ -126,26 +121,26 @@ class YieldPlugin * \param trajectory_points trajectory points * \return maximum speed */ - double max_trajectory_speed(const std::vector& trajectory_points) const; + double max_trajectory_speed(const std::vector& trajectory_points) const; /** * \brief calculates distance between trajectory points in a plan * \param trajectory_plan input trajectory plan * \return vector of relative distances between trajectory points */ - std::vector get_relative_downtracks(const cav_msgs::TrajectoryPlan& trajectory_plan) const; + std::vector get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const; /** * \brief callback for mobility request * \param msg mobility request message */ - void mobilityrequest_cb(const cav_msgs::MobilityRequestConstPtr& msg); + void mobilityrequest_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg); /** * \brief callback for bsm message * \param msg mobility bsm message */ - void bsm_cb(const cav_msgs::BSMConstPtr& msg); + void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg); /** * \brief convert a carma trajectory from ecef frame to map frame @@ -153,7 +148,7 @@ class YieldPlugin * \param ecef_trajectory carma trajectory (ecef frame) * \return vector of 2d points in map frame */ - std::vector convert_eceftrajectory_to_mappoints(const cav_msgs::Trajectory& ecef_trajectory) const; + std::vector convert_eceftrajectory_to_mappoints(const carma_v2x_msgs::msg::Trajectory& ecef_trajectory) const; /** * \brief convert a point in ecef frame (in cm) into map frame (in meters) @@ -161,7 +156,7 @@ class YieldPlugin * \param map_in_earth translate frame * \return 2d point in map frame */ - lanelet::BasicPoint2d ecef_to_map_point(const cav_msgs::LocationECEF& ecef_point) const; + lanelet::BasicPoint2d ecef_to_map_point(const carma_v2x_msgs::msg::LocationECEF& ecef_point) const; /** * \brief compose a mobility response message @@ -170,7 +165,7 @@ class YieldPlugin * \param response accept/reject to the response based on conditions * \return filled mobility response */ - cav_msgs::MobilityResponse compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const; + carma_v2x_msgs::msg::MobilityResponse compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const; /** * \brief generate a Jerk Minimizing Trajectory(JMT) with the provided start and end conditions @@ -182,7 +177,7 @@ class YieldPlugin * \param planning_time time duration of the planning * \return updated JMT trajectory */ - cav_msgs::TrajectoryPlan generate_JMT_trajectory(const cav_msgs::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time); + carma_planning_msgs::msg::TrajectoryPlan generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos, double initial_velocity, double goal_velocity, double planning_time); /** * \brief update trajectory for yielding to an incoming cooperative behavior @@ -190,7 +185,7 @@ class YieldPlugin * \param current_speed current speed of the vehicle * \return updated trajectory for cooperative behavior */ - cav_msgs::TrajectoryPlan update_traj_for_cooperative_behavior(const cav_msgs::TrajectoryPlan& original_tp, double current_speed); + carma_planning_msgs::msg::TrajectoryPlan update_traj_for_cooperative_behavior(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double current_speed); /** * \brief detect intersection point(s) of two trajectories @@ -220,23 +215,22 @@ class YieldPlugin * \param original_tp original trajectory plan * \return minumum required */ - double check_traj_for_digital_min_gap(const cav_msgs::TrajectoryPlan& original_tp) const; + 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. */ - void georeferenceCallback(const std_msgs::StringConstPtr& msg); + void georeferenceCallback(const std_msgs::msg::String::UniquePtr msg); private: carma_wm::WorldModelConstPtr wm_; YieldPluginConfig config_; - PublishPluginDiscoveryCB plugin_discovery_publisher_; MobilityResponseCB mobility_response_publisher_; LaneChangeStatusCB lc_status_publisher_; - + std::shared_ptr nh_; // flag to show if it is possible for the vehicle to accept the cooperative request bool cooperative_request_acceptable_ = false; @@ -252,16 +246,14 @@ class YieldPlugin // time between ecef trajectory points double ecef_traj_timestep_ = 0.1; - - cav_msgs::Plugin plugin_discovery_msg_; - geometry_msgs::Vector3 host_vehicle_size; + geometry_msgs::msg::Vector3 host_vehicle_size; double current_speed_; // BSM Message std::string host_bsm_id_; std::shared_ptr map_projector_; - std::string bsmIDtoString(cav_msgs::BSMCoreData bsm_core) + std::string bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core) { std::string res = ""; for (size_t i=0; i -#include -#include -#include -#include -#include -#include - -#include "yield_plugin.h" -#include "yield_config.h" - -namespace yield_plugin -{ -/** - * \brief ROS node for the YieldPlugin - */ -class YieldPluginNode -{ -public: - - /** - * \brief Entrypoint for this node - */ - void run() - { - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - carma_wm::WMListener wml; - auto wm_ = wml.getWorldModel(); - - ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); - ros::Publisher mob_resp_pub = nh.advertise("outgoing_mobility_response", 1); - ros::Publisher lc_status_pub = nh.advertise("cooperative_lane_change_status", 10); - - YieldPluginConfig config; - - pnh.param("acceleration_adjustment_factor", config.acceleration_adjustment_factor, config.acceleration_adjustment_factor); - pnh.param("collision_horizon", config.collision_horizon, config.collision_horizon); - pnh.param("min_obstacle_speed", config.min_obstacle_speed, config.min_obstacle_speed); - pnh.param("tpmin", config.tpmin, config.tpmin); - pnh.param("yield_max_deceleration", config.yield_max_deceleration, config.yield_max_deceleration); - pnh.param("x_gap", config.x_gap, config.x_gap); - pnh.param("max_stop_speed", config.max_stop_speed, config.max_stop_speed); - pnh.getParam("/vehicle_length", config.vehicle_length); - pnh.getParam("/vehicle_height", config.vehicle_height); - pnh.getParam("/vehicle_width", config.vehicle_width); - pnh.getParam("/vehicle_id", config.vehicle_id); - pnh.param("always_accept_mobility_request", config.always_accept_mobility_request, config.always_accept_mobility_request); - pnh.param("acceptable_passed_timesteps", config.acceptable_passed_timesteps, config.acceptable_passed_timesteps); - pnh.param("intervehicle_collision_distance", config.intervehicle_collision_distance, config.intervehicle_collision_distance); - pnh.param("safety_collision_time_gap", config.safety_collision_time_gap, config.safety_collision_time_gap); - pnh.param("enable_adjustable_gap", config.enable_adjustable_gap, config.enable_adjustable_gap); - pnh.param("acceptable_urgency", config.acceptable_urgency, config.acceptable_urgency); - pnh.param("speed_moving_average_window_size", config.speed_moving_average_window_size, config.speed_moving_average_window_size); - ROS_INFO_STREAM("YieldPlugin Params" << config); - - YieldPlugin worker(wm_, config, [&discovery_pub](auto msg) { discovery_pub.publish(msg); }, [&mob_resp_pub](auto msg) { mob_resp_pub.publish(msg); }, - [&lc_status_pub](auto msg) { lc_status_pub.publish(msg); }); - - ros::ServiceServer trajectory_srv_ = nh.advertiseService("yield_plugin/plan_trajectory", - &YieldPlugin::plan_trajectory_cb, &worker); - ros::Subscriber mob_request_sub = nh.subscribe("incoming_mobility_request", 5, &YieldPlugin::mobilityrequest_cb, &worker); - ros::Subscriber bsm_sub = nh.subscribe("bsm_outbound", 1, &YieldPlugin::bsm_cb, &worker); - ros::Subscriber georeference_sub = nh.subscribe("georeference", 1, &YieldPlugin::georeferenceCallback, &worker); - - ros::Timer discovery_pub_timer_ = nh.createTimer( - ros::Duration(ros::Rate(10.0)), - [&worker](const auto&) { worker.onSpin(); }); - - ros::CARMANodeHandle::spin(); - - } -}; - -} // 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 new file mode 100644 index 0000000000..c083e36945 --- /dev/null +++ b/yield_plugin/include/yield_plugin/yield_plugin_node.hpp @@ -0,0 +1,84 @@ +#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. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "yield_plugin.hpp" +#include "yield_config.hpp" + +namespace yield_plugin +{ +/** + * \brief ROS node for the YieldPlugin + */ +class YieldPluginNode : public carma_guidance_plugins::TacticalPlugin +{ +public: + + /** + * \brief Node constructor + */ + explicit YieldPluginNode(const rclcpp::NodeOptions &); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn on_configure_plugin(); + + bool get_availability() override; + + std::string get_version_id() override final; + + void plan_trajectory_callback( + std::shared_ptr srv_header, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; + +private: + + // Config + YieldPluginConfig config_; + + // Worker + std::shared_ptr worker_; + + std::string version_id_; + + // Publishers + carma_ros2_utils::PubPtr mob_resp_pub_; + carma_ros2_utils::PubPtr lc_status_pub_; + + // Subscribers + carma_ros2_utils::SubPtr mob_request_sub_; + carma_ros2_utils::SubPtr bsm_sub_; + carma_ros2_utils::SubPtr georeference_sub_; + +}; + +} // namespace yield_plugin + + + diff --git a/yield_plugin/launch/yield_plugin.launch b/yield_plugin/launch/yield_plugin.launch deleted file mode 100644 index 02919b51c3..0000000000 --- a/yield_plugin/launch/yield_plugin.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - diff --git a/yield_plugin/launch/yield_plugin.launch.py b/yield_plugin/launch/yield_plugin.launch.py new file mode 100644 index 0000000000..8bfb2c9a8f --- /dev/null +++ b/yield_plugin/launch/yield_plugin.launch.py @@ -0,0 +1,67 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA yield_plugin node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='DEBUG') + + # Get parameter file path + param_file_path = os.path.join( + get_package_share_directory('yield_plugin'), 'config/parameters.yaml') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='yield_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='yield_plugin', + plugin='yield_plugin::YieldPluginNode', + name='yield_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ param_file_path ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/yield_plugin/package.xml b/yield_plugin/package.xml index 0f1211fee9..5cc9bbced7 100644 --- a/yield_plugin/package.xml +++ b/yield_plugin/package.xml @@ -1,7 +1,7 @@ - - - - - - - - \ No newline at end of file From eb5d08dfd82ea48e672c547512d7545058b02fda Mon Sep 17 00:00:00 2001 From: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com> Date: Mon, 29 Aug 2022 12:31:40 -0400 Subject: [PATCH 095/165] basic_autonomy using defined lanelets (#1912) Description basic_autonomy is updated to use defined lanelets for lane follow maneuvers Related Issue #1863 Motivation and Context How Has This Been Tested? Integration tested on develop and 4.2.0 with Blue Lexus and White Pacifica Unit tested Co-authored-by: mishkamn Co-authored-by: Michael McConnell --- arbitrator/include/capabilities_interface.tpp | 2 +- basic_autonomy/src/basic_autonomy.cpp | 54 +++++++++++-------- .../test/test_waypoint_generation.cpp | 51 +++++++++++++++++- basic_autonomy_ros2/src/basic_autonomy.cpp | 41 +++++++++++++- .../test/test_waypoint_generation.cpp | 51 +++++++++++++++++- .../include/route_following_plugin_config.hpp | 3 +- .../src/route_following_plugin.cpp | 3 +- 7 files changed, 177 insertions(+), 28 deletions(-) diff --git a/arbitrator/include/capabilities_interface.tpp b/arbitrator/include/capabilities_interface.tpp index 0cf8f734b6..a77a1bf1b6 100644 --- a/arbitrator/include/capabilities_interface.tpp +++ b/arbitrator/include/capabilities_interface.tpp @@ -35,7 +35,7 @@ namespace arbitrator for (auto i = topics.begin(); i != topics.end(); i++) { - auto sc = nh_->create_client("plugins/" + *i); + auto sc = nh_->create_client(*i); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "found client: " << *i); std::shared_future> resp = sc->async_send_request(msg); diff --git a/basic_autonomy/src/basic_autonomy.cpp b/basic_autonomy/src/basic_autonomy.cpp index 3dec684ad5..b4f01c2499 100644 --- a/basic_autonomy/src/basic_autonomy.cpp +++ b/basic_autonomy/src/basic_autonomy.cpp @@ -69,40 +69,52 @@ namespace basic_autonomy const carma_wm::WorldModelConstPtr &wm, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config, std::unordered_set &visited_lanelets) { - if(maneuver.type != cav_msgs::Maneuver::LANE_FOLLOWING){ + if (maneuver.type != cav_msgs::Maneuver::LANE_FOLLOWING) + { throw std::invalid_argument("Create_lanefollow called on a maneuver type which is not LANE_FOLLOW"); } std::vector points_and_target_speeds; cav_msgs::LaneFollowingManeuver lane_following_maneuver = maneuver.lane_following_maneuver; - auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist + detailed_config.buffer_ending_downtrack, true, true); - - //TODO: This fix is temporary and only applies on lane follow maneuvers from Platooning Strategic Plugin IHP. - // A general fix will be implemented soon. (issue #1863) - bool lanelets_defined = !maneuver.lane_following_maneuver.lane_ids.empty(); - ROS_DEBUG_STREAM("lanelets_defined: " << lanelets_defined); - bool isFromPlatooning = maneuver.lane_following_maneuver.parameters.planning_strategic_plugin == "PlatooningStrategicIHPPlugin"; - ROS_DEBUG_STREAM("isFromPlatooning: " << isFromPlatooning); + if (maneuver.lane_following_maneuver.lane_ids.empty()) + { + throw std::invalid_argument("No lanelets are defined for lanefollow maneuver"); + } - if (lanelets_defined && isFromPlatooning) + std::vector lanelets = { wm->getMap()->laneletLayer.get(stoi(lane_following_maneuver.lane_ids[0]))}; // Accept first lanelet reguardless + for (size_t i = 1; i < lane_following_maneuver.lane_ids.size(); i++) // Iterate over remaining lanelets and check if they are followers of the previous lanelet { - lanelets = {}; - int lane_id = stoi(maneuver.lane_following_maneuver.lane_ids[0]); - ROS_DEBUG_STREAM("extracted id: " << lane_id); - lanelet::ConstLanelet new_lanelet = wm->getMap()->laneletLayer.get(lane_id); - lanelets.push_back(new_lanelet); + auto ll_id = lane_following_maneuver.lane_ids[i]; + int cur_id = stoi(ll_id); + auto cur_ll = wm->getMap()->laneletLayer.get(cur_id); + auto following_lanelets = wm->getMapRoutingGraph()->following(lanelets.back()); + + bool is_follower = false; + for (auto follower_ll : following_lanelets ) + { + if (follower_ll.id() == cur_ll.id()) + { + is_follower = true; + break; + } + } - if (maneuver.lane_following_maneuver.lane_ids.size()>1) + if (!is_follower) { - lane_id = stoi(maneuver.lane_following_maneuver.lane_ids[1]); - ROS_DEBUG_STREAM("more extracted id: " << lane_id); - new_lanelet = wm->getMap()->laneletLayer.get(lane_id); - lanelets.push_back(new_lanelet); + throw std::invalid_argument("Invalid list of lanelets they are not followers"); } - } + lanelets.push_back(cur_ll); // Keep lanelet + } + + // Add extra lanelet to ensure there are sufficient points for buffer + auto extra_following_lanelets = wm->getMapRoutingGraph()->following(lanelets.back()); + if (!extra_following_lanelets.empty()) + { + lanelets.push_back(extra_following_lanelets[0]); + } if (lanelets.empty()) { diff --git a/basic_autonomy/test/test_waypoint_generation.cpp b/basic_autonomy/test/test_waypoint_generation.cpp index b753a676ff..7703c7b916 100644 --- a/basic_autonomy/test/test_waypoint_generation.cpp +++ b/basic_autonomy/test/test_waypoint_generation.cpp @@ -779,12 +779,61 @@ namespace basic_autonomy std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - EXPECT_EQ(visited_lanelets.size(), 5); + EXPECT_EQ(visited_lanelets.size(), 3); EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set EXPECT_TRUE(visited_lanelets.find(1200) != visited_lanelets.end()); // new lanelets added to the set with the new maneuver } + TEST(BasicAutonomyTest, lanefollow_defined_lanelets) + { + + double starting_downtrack = 0; + cav_msgs::VehicleState ending_state; + std::string trajectory_type = "lane_follow"; + waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 0, 0); + waypoint_generation::DetailedTrajConfig detailed_config = waypoint_generation::compose_detailed_trajectory_config(0, 0, 0, 0, 0, 5, 0, 0, 20); + + lanelet::Id id1 = 1100; + std::unordered_set visited_lanelets; + visited_lanelets.insert(id1); + std::shared_ptr wm = std::make_shared(); + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); + wm->setMap(map); + carma_wm::test::setSpeedLimit(15_mph, wm); + + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + cav_msgs::Maneuver maneuver; + maneuver.type = cav_msgs::Maneuver::LANE_FOLLOWING; + maneuver.lane_following_maneuver.lane_ids = {}; + maneuver.lane_following_maneuver.start_dist = 5.0; + maneuver.lane_following_maneuver.start_time = ros::Time(0.0); + maneuver.lane_following_maneuver.start_speed = 0.0; + + maneuver.lane_following_maneuver.end_dist = 14.98835712; + maneuver.lane_following_maneuver.end_speed = 6.7056; + maneuver.lane_following_maneuver.end_time = ros::Time(4.4704); + + // No defined lanelet ids in the maneuver msg + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, detailed_config, visited_lanelets), std::invalid_argument); + + // Defined lanelet ids in the maneuver msg + maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1200)); + std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, detailed_config, visited_lanelets); + EXPECT_EQ(visited_lanelets.size(), 2); + EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set + EXPECT_TRUE(visited_lanelets.find(1200) != visited_lanelets.end()); // new lanelets added to the set with the new maneuver + + // Non-following lanelet id in the maneuver msg + maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1202)); + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, + detailed_config, visited_lanelets), std::invalid_argument); + } + } //basic_autonomy namespace // Run all the tests diff --git a/basic_autonomy_ros2/src/basic_autonomy.cpp b/basic_autonomy_ros2/src/basic_autonomy.cpp index bc1f666af9..6c736e7639 100644 --- a/basic_autonomy_ros2/src/basic_autonomy.cpp +++ b/basic_autonomy_ros2/src/basic_autonomy.cpp @@ -75,9 +75,46 @@ namespace basic_autonomy std::vector points_and_target_speeds; carma_planning_msgs::msg::LaneFollowingManeuver lane_following_maneuver = maneuver.lane_following_maneuver; - - auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist + detailed_config.buffer_ending_downtrack, true, true); + if (maneuver.lane_following_maneuver.lane_ids.empty()) + { + throw std::invalid_argument("No lanelets are defined for lanefollow maneuver"); + } + + std::vector lanelets = { wm->getMap()->laneletLayer.get(stoi(lane_following_maneuver.lane_ids[0]))}; // Accept first lanelet reguardless + for (size_t i = 1; i < lane_following_maneuver.lane_ids.size(); i++) // Iterate over remaining lanelets and check if they are followers of the previous lanelet + { + auto ll_id = lane_following_maneuver.lane_ids[i]; + int cur_id = stoi(ll_id); + auto cur_ll = wm->getMap()->laneletLayer.get(cur_id); + auto following_lanelets = wm->getMapRoutingGraph()->following(lanelets.back()); + + bool is_follower = false; + for (auto follower_ll : following_lanelets ) + { + if (follower_ll.id() == cur_ll.id()) + { + is_follower = true; + break; + } + } + + if (!is_follower) + { + throw std::invalid_argument("Invalid list of lanelets they are not followers"); + } + + lanelets.push_back(cur_ll); // Keep lanelet + + } + + // Add extra lanelet to ensure there are sufficient points for buffer + auto extra_following_lanelets = wm->getMapRoutingGraph()->following(lanelets.back()); + if (!extra_following_lanelets.empty()) + { + lanelets.push_back(extra_following_lanelets[0]); + } + if (lanelets.empty()) { RCLCPP_ERROR_STREAM(rclcpp::get_logger(BASIC_AUTONOMY_LOGGER), "Detected no lanelets between starting downtrack: "<< starting_downtrack << ", and lane_following_maneuver.end_dist: "<< lane_following_maneuver.end_dist); diff --git a/basic_autonomy_ros2/test/test_waypoint_generation.cpp b/basic_autonomy_ros2/test/test_waypoint_generation.cpp index 4d6b0b5686..d424d00257 100644 --- a/basic_autonomy_ros2/test/test_waypoint_generation.cpp +++ b/basic_autonomy_ros2/test/test_waypoint_generation.cpp @@ -778,12 +778,61 @@ TEST(BasicAutonomyTest, get_nearest_basic_point_index) std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - EXPECT_EQ(visited_lanelets.size(), 5); + EXPECT_EQ(visited_lanelets.size(), 3); EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set EXPECT_TRUE(visited_lanelets.find(1200) != visited_lanelets.end()); // new lanelets added to the set with the new maneuver } + TEST(BasicAutonomyTest, lanefollow_defined_lanelets) + { + + double starting_downtrack = 0; + carma_planning_msgs::msg::VehicleState ending_state; + std::string trajectory_type = "lane_follow"; + waypoint_generation::GeneralTrajConfig general_config = waypoint_generation::compose_general_trajectory_config(trajectory_type, 0, 0); + waypoint_generation::DetailedTrajConfig detailed_config = waypoint_generation::compose_detailed_trajectory_config(0, 0, 0, 0, 0, 5, 0, 0, 20); + + lanelet::Id id1 = 1100; + std::unordered_set visited_lanelets; + visited_lanelets.insert(id1); + std::shared_ptr wm = std::make_shared(); + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); + wm->setMap(map); + carma_wm::test::setSpeedLimit(15_mph, wm); + + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + carma_planning_msgs::msg::Maneuver maneuver; + maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver.lane_following_maneuver.lane_ids = {}; + maneuver.lane_following_maneuver.start_dist = 5.0; + maneuver.lane_following_maneuver.start_time = rclcpp::Time(0.0); + maneuver.lane_following_maneuver.start_speed = 0.0; + + maneuver.lane_following_maneuver.end_dist = 14.98835712; + maneuver.lane_following_maneuver.end_speed = 6.7056; + maneuver.lane_following_maneuver.end_time = rclcpp::Time(4.4704*1e9); // 4.4704 seconds converted to nanoseconds + + // No defined lanelet ids in the maneuver msg + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, detailed_config, visited_lanelets), std::invalid_argument); + + // Defined lanelet ids in the maneuver msg + maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1200)); + std::vector points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, detailed_config, visited_lanelets); + EXPECT_EQ(visited_lanelets.size(), 3); + EXPECT_TRUE(visited_lanelets.find(id1) != visited_lanelets.end()); // the lanelet previously in the visited lanelet set + EXPECT_TRUE(visited_lanelets.find(1200) != visited_lanelets.end()); // new lanelets added to the set with the new maneuver + + // Non-following lanelet id in the maneuver msg + maneuver.lane_following_maneuver.lane_ids.push_back(std::to_string(1202)); + EXPECT_THROW(basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, + starting_downtrack, wm, general_config, + detailed_config, visited_lanelets), std::invalid_argument); + } + } // namespace basic_autonomy // Run all the tests diff --git a/route_following_plugin/include/route_following_plugin_config.hpp b/route_following_plugin/include/route_following_plugin_config.hpp index 16e4a8e880..91c561b06a 100644 --- a/route_following_plugin/include/route_following_plugin_config.hpp +++ b/route_following_plugin/include/route_following_plugin_config.hpp @@ -30,7 +30,7 @@ namespace route_following_plugin //Tactical plugin being used for planning lane change std::string lane_change_plugin_ = "cooperative_lanechange"; std::string stop_and_wait_plugin_ = "stop_and_wait_plugin"; - + std::string vehicle_id = "vehicle_id"; std::string lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"; double route_end_point_buffer_ = 10.0; double accel_limit_ = 2.0; @@ -51,6 +51,7 @@ namespace route_following_plugin << "lateral_accel_limit_: " << c.lateral_accel_limit_ << std::endl << "stopping_accel_limit_multiplier_: " << c.stopping_accel_limit_multiplier_ << std::endl << "min_maneuver_length_: " << c.min_maneuver_length_ << std::endl + << "vehicle_id: " << c.vehicle_id << std::endl << "}" << std::endl; return output; } diff --git a/route_following_plugin/src/route_following_plugin.cpp b/route_following_plugin/src/route_following_plugin.cpp index 5dd415a80c..77a4101431 100644 --- a/route_following_plugin/src/route_following_plugin.cpp +++ b/route_following_plugin/src/route_following_plugin.cpp @@ -96,7 +96,8 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id config_.accel_limit_ = declare_parameter("vehicle_acceleration_limit", config_.accel_limit_); config_.lateral_accel_limit_ = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit_); config_.stopping_accel_limit_multiplier_ = declare_parameter("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_); - config_.min_maneuver_length_ = declare_parameter("vehicle_id", config_.min_maneuver_length_); + config_.vehicle_id = declare_parameter("vehicle_id", config_.vehicle_id); + config_.min_maneuver_length_ = declare_parameter("min_maneuver_length", config_.min_maneuver_length_); } carma_ros2_utils::CallbackReturn RouteFollowingPlugin::on_configure_plugin() From 08fdd93711c1fd2c9e2b3e666ac340db47631abd Mon Sep 17 00:00:00 2001 From: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com> Date: Tue, 30 Aug 2022 08:37:19 -0400 Subject: [PATCH 096/165] update ihp lanechange conditions (#1916) Description IHP Strategic Platooning plugin is updated to use more general conditions for checking a lane change. Related Issue #1864 Co-authored-by: Michael McConnell --- .../platoon_strategic_ihp.h | 11 + .../map/town01_vector_map_lane_change.osm | 55348 ++++++++++++++++ .../src/platoon_strategic_ihp.cpp | 117 +- .../test/test_platoon_manager_front_join.cpp | 53 +- 4 files changed, 55477 insertions(+), 52 deletions(-) create mode 100755 platooning_strategic_IHP/resource/map/town01_vector_map_lane_change.osm diff --git a/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_ihp.h b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_ihp.h index 475af88ae7..8667cd85ca 100644 --- a/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_ihp.h +++ b/platooning_strategic_IHP/include/platoon_strategic_ihp/platoon_strategic_ihp.h @@ -756,6 +756,16 @@ namespace platoon_strategic_ihp void mob_resp_cb_preparetojoin(const carma_v2x_msgs::msg::MobilityResponse& msg); + /** + * \brief Function to check if lanechange is possible + * + * \param start_lanelet_id start lanelet id + * \param target_lanelet_id start lanelet id + * + * \return true or false + */ + bool is_lanechange_possible(lanelet::Id start_lanelet_id, lanelet::Id target_lanelet_id); + // Pointer for map projector std::shared_ptr map_projector_; @@ -846,5 +856,6 @@ namespace platoon_strategic_ihp // Unit Test Accessors FRIEND_TEST(PlatoonStrategicIHPPlugin, platoon_info_pub_front); + FRIEND_TEST(PlatoonStrategicIHPPlugin, is_lanechange_possible); }; } diff --git a/platooning_strategic_IHP/resource/map/town01_vector_map_lane_change.osm b/platooning_strategic_IHP/resource/map/town01_vector_map_lane_change.osm new file mode 100755 index 0000000000..85a1b7ed45 --- /dev/null +++ b/platooning_strategic_IHP/resource/map/town01_vector_map_lane_change.osm @@ -0,0 +1,55348 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp index db480a4264..52fd75d793 100644 --- a/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp +++ b/platooning_strategic_IHP/src/platoon_strategic_ihp.cpp @@ -44,12 +44,10 @@ namespace platoon_strategic_ihp RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Top of PlatoonStrategicIHP ctor."); std::string hostStaticId = config_.vehicleID; //static ID for this vehicle pm_.HostMobilityId = hostStaticId; - // construct platoon member for host vehicle as the first element in the vector, since it starts life as a solo vehicle long cur_t = timer_factory_->now().nanoseconds()/1000000; // time in millisecond PlatoonMember hostVehicleMember = PlatoonMember(hostStaticId, 0.0, 0.0, 0.0, 0.0, cur_t); pm_.host_platoon_.push_back(hostVehicleMember); - plugin_discovery_msg_.name = "platoon_strategic_ihp"; plugin_discovery_msg_.version_id = "v1.0"; plugin_discovery_msg_.available = true; @@ -2958,16 +2956,6 @@ namespace platoon_strategic_ihp prevHeartBeatTime_ = timer_factory_->now().nanoseconds() / 1000000; } - // // Task 3: plan time out - // if (pm_.current_plan.valid) - // { - // bool isCurrentPlanTimeout = ((timer_factory_->now().nanoseconds() / 1000000 - pm_.current_plan.planStartTime) > NEGOTIATION_TIMEOUT); - // if (isCurrentPlanTimeout) - // { - // RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Give up waiting on plan with planId: " << pm_.current_plan.planId << "; stay in LEADWITHOPERATION"); - // pm_.current_plan.valid = false; - // } - // } // Task 4: STATUS msgs bool hasFollower = pm_.getHostPlatoonSize() > 1 || config_.test_cutin_join; @@ -3236,7 +3224,19 @@ namespace platoon_strategic_ihp lanelet::BasicPoint2d current_loc(pose_msg_.pose.position.x, pose_msg_.pose.position.y); // *** get the actually closest lanelets that relate to current location (n=10) ***// - auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 10); + auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, current_loc, 10); + + lanelet::ConstLanelet current_lanelet; + + // To avoid overlapping lanelets, compare the nearest lanelets with the route + for (auto llt: current_lanelets) + { + if (wm_->getRoute()->contains(llt.second)) + { + current_lanelet = llt.second; + break; + } + } // raise warn if no path was found if(current_lanelets.size() == 0) @@ -3248,23 +3248,6 @@ namespace platoon_strategic_ihp // locate lanelet on shortest path auto shortest_path = wm_->getRoute()->shortestPath(); // find path amoung route - lanelet::ConstLanelet current_lanelet; - int last_lanelet_index = -1; - for (auto llt : current_lanelets) - { - if (boost::geometry::within(current_loc, llt.second.polygon2d())) - { - int potential_index = findLaneletIndexFromPath(llt.second.id(), shortest_path); // usage: findLaneletIndexFromPath(target_id, lanelet2_path) - if (potential_index != -1) - { - last_lanelet_index = potential_index; - current_lanelet = shortest_path[last_lanelet_index]; // find lanelet2 from map that corresponse to the path - break; - } - } - } - - // read status data double current_progress = wm_->routeTrackPos(current_loc).downtrack; double speed_progress = current_speed_; @@ -3290,7 +3273,6 @@ namespace platoon_strategic_ihp time_progress = req.prior_plan.planning_completion_time; int end_lanelet = 0; updateCurrentStatus(req.prior_plan.maneuvers.back(), speed_progress, current_progress, end_lanelet); - last_lanelet_index = findLaneletIndexFromPath(end_lanelet, shortest_path); } RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Starting Loop"); @@ -3300,7 +3282,10 @@ namespace platoon_strategic_ihp RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "in mvr callback safeToLaneChange: " << safeToLaneChange_); // Note: Use current_lanlet list (which was determined based on vehicle pose) to find current lanelet ID. - long current_lanelet_id = current_lanelets[0].second.id(); + lanelet::Id current_lanelet_id = current_lanelet.id(); + + + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "current_lanelet_id: " << current_lanelet_id); // lane change maneuver if (safeToLaneChange_) @@ -3348,24 +3333,14 @@ namespace platoon_strategic_ihp RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "The target cutin pose is not on a valid lanelet. So no lane change!"); break; } - int target_lanelet_id = target_lanelets[0].second.id(); + lanelet::Id target_lanelet_id = target_lanelets[0].second.id(); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "target_lanelet_id: " << target_lanelet_id); lanelet::ConstLanelet starting_lanelet = wm_->getMap()->laneletLayer.get(current_lanelet_id); lanelet::ConstLanelet ending_lanelet = wm_->getMap()->laneletLayer.get(target_lanelet_id); - auto relation = wm_->getMapRoutingGraph()->routingRelation(starting_lanelet, ending_lanelet); - bool lanechangePossible = false; - // TODO: Assuming a lane change is only needed from an adjacent left/right lanelet. Only valid for IHP platooning. - // Need to generalize in future. Refer to issue #1864 - if (relation == lanelet::routing::RelationType::Left || relation == lanelet::routing::RelationType::Right) - { - lanechangePossible = true; - } - else - { - lanechangePossible = false; - } + bool lanechangePossible = is_lanechange_possible(current_lanelet_id, target_lanelet_id); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "lanechangePossible: " << lanechangePossible); if (lanechangePossible) { @@ -3387,12 +3362,11 @@ namespace platoon_strategic_ihp // read lane change maneuver end time as time progress time_progress = resp.new_plan.maneuvers.back().lane_change_maneuver.end_time; speed_progress = target_speed; - if(current_progress >= total_maneuver_length || last_lanelet_index == static_cast(shortest_path.size()) - 1) + if(current_progress >= total_maneuver_length) { break; } - ++last_lanelet_index; } } @@ -3423,11 +3397,10 @@ namespace platoon_strategic_ihp current_progress += dist_diff; time_progress = resp.new_plan.maneuvers.back().lane_following_maneuver.end_time; speed_progress = target_speed; - if(current_progress >= total_maneuver_length || last_lanelet_index == static_cast(shortest_path.size()) - 1) + if(current_progress >= total_maneuver_length) { break; } - ++last_lanelet_index; } } } @@ -3459,11 +3432,10 @@ namespace platoon_strategic_ihp current_progress += dist_diff; time_progress = resp.new_plan.maneuvers.back().lane_following_maneuver.end_time; speed_progress = target_speed; - if(current_progress >= total_maneuver_length || last_lanelet_index == static_cast(shortest_path.size()) - 1) + if(current_progress >= total_maneuver_length) { break; } - ++last_lanelet_index; } } @@ -3497,6 +3469,49 @@ namespace platoon_strategic_ihp return true; } + bool PlatoonStrategicIHPPlugin::is_lanechange_possible(lanelet::Id start_lanelet_id, lanelet::Id target_lanelet_id) + { + lanelet::ConstLanelet starting_lanelet = wm_->getMap()->laneletLayer.get(start_lanelet_id); + lanelet::ConstLanelet ending_lanelet = wm_->getMap()->laneletLayer.get(target_lanelet_id); + lanelet::ConstLanelet current_lanelet = starting_lanelet; + bool shared_boundary_found = false; + + while(!shared_boundary_found) + { + //Assumption- Adjacent lanelets share lane boundary + if(current_lanelet.leftBound() == ending_lanelet.rightBound()) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lanelet " << std::to_string(current_lanelet.id()) << " shares left boundary with " << std::to_string(ending_lanelet.id())); + shared_boundary_found = true; + } + else if(current_lanelet.rightBound() == ending_lanelet.leftBound()) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platoon_strategic_ihp"), "Lanelet " << std::to_string(current_lanelet.id()) << " shares right boundary with " << std::to_string(ending_lanelet.id())); + shared_boundary_found = true; + } + + else + { + //If there are no following lanelets on route, lanechange should be completing before reaching it + if(wm_->getMapRoutingGraph()->following(current_lanelet, false).empty()) + { + //In this case we have reached a lanelet which does not have a routable lanelet ahead + isn't adjacent to the lanelet where lane change ends + return false; + } + + current_lanelet = wm_->getMapRoutingGraph()->following(current_lanelet, false).front(); + if(current_lanelet.id() == starting_lanelet.id()) + { + //Looped back to starting lanelet + return false; + } + } + } + + + return true; + } + //--------------------------------------------------------------------------// void PlatoonStrategicIHPPlugin::setConfig(const PlatoonPluginConfig& config) diff --git a/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp b/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp index 7b84499877..f9cf3de707 100644 --- a/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp +++ b/platooning_strategic_IHP/test/test_platoon_manager_front_join.cpp @@ -35,6 +35,17 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include + +using carma_ros2_utils::timers::testing::TestTimer; +using carma_ros2_utils::timers::testing::TestTimerFactory; namespace platoon_strategic_ihp { @@ -208,7 +219,7 @@ TEST(PlatoonStrategicIHPPlugin, cutin_test_leader_2) EXPECT_EQ(resp, MobilityRequestResponse::ACK); } -//UCLA: Cut-in front test for joining vehicle. Test the transition "leader --> prepare to join" +// UCLA: Cut-in front test for joining vehicle. Test the transition "leader --> prepare to join" TEST(PlatoonStrategicIHPPlugin, cutin_test_join_1) { PlatoonPluginConfig config; @@ -378,4 +389,44 @@ TEST(PlatoonManagerTestFrontJoin, test4_front) EXPECT_EQ(0, res); } +TEST(PlatoonStrategicIHPPlugin, is_lanechange_possible) + { + std::string path = ament_index_cpp::get_package_share_directory("platoon_strategic_ihp"); + std::string file = "/resource/map/town01_vector_map_lane_change.osm"; + file = path.append(file); + int projector_type = 0; + std::string target_frame; + lanelet::ErrorMessages load_errors; + lanelet::io_handlers::AutowareOsmParser::parseMapParams(file, &projector_type, &target_frame); + lanelet::projection::LocalFrameProjector local_projector(target_frame.c_str()); + lanelet::LaneletMapPtr map = lanelet::load(file, local_projector, &load_errors); + if (map->laneletLayer.size() == 0) + { + FAIL() << "Input map does not contain any lanelets"; + } + std::shared_ptr cmw = std::make_shared(); + cmw->carma_wm::CARMAWorldModel::setMap(map); + //Set Route + lanelet::Id start_id = 106; + lanelet::Id end_id = 110; + carma_wm::test::setRouteByIds({start_id, end_id}, cmw); + cmw->carma_wm::CARMAWorldModel::setMap(map); + + PlatoonPluginConfig config; + + PlatoonStrategicIHPPlugin plugin(cmw, config, [&](auto) {}, [&](auto) {}, [&](auto) {}, [&](auto) {}, + std::make_shared()); + + lanelet::Id target_id1 = 111; + bool lanechange_possible = plugin.is_lanechange_possible(start_id, target_id1); + + EXPECT_TRUE(lanechange_possible); + + lanelet::Id target_id2 = 146; + bool lanechange_possible2 = plugin.is_lanechange_possible(start_id, target_id2); + EXPECT_FALSE(lanechange_possible2); + + + } + } // Namespace required for FRIEND_TEST to allow access to private members From 3d20a1250c78c59b83a060afa30267f40c99fccd Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 30 Aug 2022 08:38:32 -0400 Subject: [PATCH 097/165] sci_tactical_plugin port to ROS2 (#1915) Description This PR includes updates to port the sci_tactical_plugin from ROS1 to ROS2. Related Issue #1906 Motivation and Context How Has This Been Tested? All unit tests pass, and local integration tests have been conducted. Co-authored-by: Michael McConnell --- carma/launch/guidance.launch | 3 - carma/launch/plugins.launch.py | 24 ++ carma_wm_ros2/src/MapUpdateLoggerNode.cpp | 2 +- .../CMakeLists.txt | 103 +++---- .../config/parameters.yaml | 2 +- ...> stop_controlled_intersection_config.hpp} | 11 +- ...> stop_controlled_intersection_plugin.hpp} | 119 ++++---- ...stop_controlled_intersection_plugin_node.h | 98 ------- ...rolled_intersection_tactical_plugin.launch | 20 -- ...led_intersection_tactical_plugin.launch.py | 68 +++++ .../package.xml | 38 ++- .../src/main.cpp | 19 +- ...ontrolled_intersection_tactical_plugin.cpp | 258 +++++++++++------- .../test/test_sci_tactical_plugin.cpp | 243 +++++++++-------- .../config/guidance_controller_config.yaml | 3 +- 15 files changed, 513 insertions(+), 498 deletions(-) rename stop_controlled_intersection_tactical_plugin/include/{stop_controlled_intersection_config.h => stop_controlled_intersection_config.hpp} (91%) rename stop_controlled_intersection_tactical_plugin/include/{stop_controlled_intersection_plugin.h => stop_controlled_intersection_plugin.hpp} (64%) delete mode 100644 stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h delete mode 100644 stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch create mode 100644 stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch.py diff --git a/carma/launch/guidance.launch b/carma/launch/guidance.launch index 99918ed96b..f38a6b155a 100644 --- a/carma/launch/guidance.launch +++ b/carma/launch/guidance.launch @@ -149,9 +149,6 @@ - - - diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index a604c77c84..be2e965cb3 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -70,6 +70,9 @@ def generate_launch_description(): platoon_tactical_ihp_param_file = os.path.join( get_package_share_directory('platooning_tactical_plugin'), 'config/parameters.yaml') + + stop_controlled_intersection_tactical_plugin_file_path = os.path.join( + get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') @@ -246,6 +249,27 @@ def generate_launch_description(): vehicle_config_param_file, light_controlled_intersection_tactical_plugin_param_file ] + ), + ComposableNode( + package='stop_controlled_intersection_tactical_plugin', + plugin='stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin', + name='stop_controlled_intersection_tactical_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('stop_controlled_intersection_tactical_plugin', env_log_levels) } + ], + 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" ] ), + ("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" ] ) + ], + parameters=[ + stop_controlled_intersection_tactical_plugin_file_path, + vehicle_config_param_file + ] ) ] ) diff --git a/carma_wm_ros2/src/MapUpdateLoggerNode.cpp b/carma_wm_ros2/src/MapUpdateLoggerNode.cpp index 344cad561a..02181ea0dd 100644 --- a/carma_wm_ros2/src/MapUpdateLoggerNode.cpp +++ b/carma_wm_ros2/src/MapUpdateLoggerNode.cpp @@ -58,7 +58,7 @@ class MapUpdateLogger : public rclcpp::Node * * \param id_reg_pair The pair to convert * - * \return A corresponding carma_debug_msgs::LaneletIdRegulatoryElementPair + * \return A corresponding carma_debug_ros2_msgs::msg::LaneletIdRegulatoryElementPair */ carma_debug_ros2_msgs::msg::LaneletIdRegulatoryElementPair pairToDebugMessage(const std::pair& id_reg_pair); diff --git a/stop_controlled_intersection_tactical_plugin/CMakeLists.txt b/stop_controlled_intersection_tactical_plugin/CMakeLists.txt index 8de3b9d771..f7ddf068e7 100644 --- a/stop_controlled_intersection_tactical_plugin/CMakeLists.txt +++ b/stop_controlled_intersection_tactical_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2021 LEIDOS. +# 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 @@ -14,93 +14,64 @@ # the License. # -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(stop_controlled_intersection_tactical_plugin) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -set(CATKIN_DEPS - roscpp - std_msgs - cav_msgs - cav_srvs - carma_utils - trajectory_utils - autoware_msgs - carma_wm - tf - tf2 - tf2_geometry_msgs - basic_autonomy - carma_debug_msgs -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -## Find catkin macros and libraries -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) +# Name build targets +set(node_exec stop_controlled_intersection_tactical_plugin_exec) +set(node_lib stop_controlled_intersection_tactical_plugin_lib) ## System dependencies are found with CMake's conventions -find_package(Boost) -find_package(Eigen3 REQUIRED) - -################################### -## catkin specific configuration ## -################################### - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost EIGEN3 -) +find_package(Boost REQUIRED) ########### ## Build ## ########### include_directories( - include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} + include ) -add_executable( ${PROJECT_NAME} - src/main.cpp) - -add_library(stop_controlled_intersection_tactical_library - src/stop_controlled_intersection_tactical_plugin.cpp) +# Build +ament_auto_add_library(${node_lib} SHARED + src/stop_controlled_intersection_tactical_plugin.cpp +) -add_dependencies(stop_controlled_intersection_tactical_library ${catkin_EXPORTED_TARGETS}) -target_link_libraries(stop_controlled_intersection_tactical_library ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -target_link_libraries(${PROJECT_NAME} stop_controlled_intersection_tactical_library) +ament_auto_add_executable(${node_exec} + src/main.cpp +) -############# -## Install ## -############# +# Register component +rclcpp_components_register_nodes(${node_lib} "stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin") -install(DIRECTORY include - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# All locally created targets will need to be manually linked +# ament auto will handle linking of external dependencies +target_link_libraries(${node_exec} + ${node_lib} ) -## Install C++ -install(TARGETS ${PROJECT_NAME} stop_controlled_intersection_tactical_library - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# Testing +if(BUILD_TESTING) -## Install Other Resources -install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable -############# -## Testing ## -############# -catkin_add_gmock(${PROJECT_NAME}-test - test/test_sci_tactical_plugin.cpp - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) -target_link_libraries(${PROJECT_NAME}-test stop_controlled_intersection_tactical_library ${catkin_LIBRARIES}) \ No newline at end of file + ament_add_gtest(test_sci_tactical_plugin test/test_sci_tactical_plugin.cpp) + ament_target_dependencies(test_sci_tactical_plugin ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) + target_link_libraries(test_sci_tactical_plugin ${node_lib}) + +endif() + +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) diff --git a/stop_controlled_intersection_tactical_plugin/config/parameters.yaml b/stop_controlled_intersection_tactical_plugin/config/parameters.yaml index 1c7abd885f..216fad9c87 100644 --- a/stop_controlled_intersection_tactical_plugin/config/parameters.yaml +++ b/stop_controlled_intersection_tactical_plugin/config/parameters.yaml @@ -1,6 +1,6 @@ # Double: The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions # Units: m -cernterline_sampling_spacing: 1.0 +centerline_sampling_spacing: 1.0 trajectory_time_length: 6.0 # Trajectory length in seconds curve_resample_step_size: 1.0 # Curve re-sampling step size in m curvature_moving_average_window_size: 9 # Size of the window used in the moving average filter to smooth the computed curvature diff --git a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_config.h b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_config.hpp similarity index 91% rename from stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_config.h rename to stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_config.hpp index 3a338f9df3..2b5f5e6926 100644 --- a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_config.h +++ b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_config.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2021 LEIDOS. + * 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 @@ -18,6 +18,8 @@ #include +namespace stop_controlled_intersection_tactical_plugin +{ /** * \brief Stuct containing the algorithm configuration values for the StopControlledIntersectionTacticalPlugin */ @@ -25,7 +27,6 @@ struct StopControlledIntersectionTacticalPluginConfig { double trajectory_time_length = 6.0; // Trajectory length in seconds double curve_resample_step_size = 1.0; // Curve re-sampling step size in m - bool publish_debug = false; double centerline_sampling_spacing = 1.0; // The gap in meters between points sampled from the lanelet centerlines for planning trajectory positions int curvature_moving_average_window_size = 9; // Size of the window used in the moving average filter to smooth the curvature profile // computed curvature and output speeds @@ -38,7 +39,7 @@ struct StopControlledIntersectionTacticalPluginConfig output <<"StopControlledIntersectionTacticalPluginConfig { " < -#include -#include -#include +#include +#include +#include +#include #include -#include #include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include +#include "stop_controlled_intersection_config.hpp" #include -#include -#include -#include -#include -#include -#include -#include - +#include +#include +#include +#include +#include +#include +#include /** * \brief Macro definition to enable easier access to fields shared across the maneuver types @@ -46,15 +45,14 @@ * \return Expands to an expression (in the form of chained ternary operators) that evalutes to the desired field */ #define GET_MANEUVER_PROPERTY(mvr, property)\ - (((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ - ((mvr).type == cav_msgs::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ + (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\ + ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\ throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type))))))) -namespace stop_controlled_intersection_transit_plugin +namespace stop_controlled_intersection_tactical_plugin { -using PublishPluginDiscoveryCB = std::function; using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; /** @@ -62,30 +60,13 @@ using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair; * */ -class StopControlledIntersectionTacticalPlugin +class StopControlledIntersectionTacticalPlugin : public carma_guidance_plugins::TacticalPlugin { public: - /** - * \brief Constructor - * - * \param wm Pointer to intialized instance of the carma world model for accessing semantic map data - * \param config The configuration to be used for this object - * \param plugin_discovery_publisher Callback which will publish the current plugin discovery state - * \param debug_publisher Callback which will publish a debug message. The callback defaults to no-op. - */ - StopControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm,const StopControlledIntersectionTacticalPluginConfig& config, - const PublishPluginDiscoveryCB &plugin_discovery_publisher); - /** - * \brief Service callback for trajectory planning - * - * \param req The service request - * \param resp The service response - * - * \return True if success. False otherwise - */ - bool plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req, cav_srvs::PlanTrajectoryResponse& resp); - + // brief Constructor + explicit StopControlledIntersectionTacticalPlugin(const rclcpp::NodeOptions &options); + /** * \brief Converts a set of requested stop controlled intersection maneuvers to point speed limit pairs. * @@ -96,9 +77,9 @@ class StopControlledIntersectionTacticalPlugin * * \return List of centerline points paired with target speeds */ - std::vector maneuvers_to_points(const std::vector& maneuvers, + std::vector maneuvers_to_points(const std::vector& maneuvers, const carma_wm::WorldModelConstPtr& wm, - const cav_msgs::VehicleState& state); + const carma_planning_msgs::msg::VehicleState& state); /** * \brief Creates a speed profile according to case one of the stop controlled intersection, where the vehicle accelerates and then decelerates to a stop. @@ -112,8 +93,8 @@ class StopControlledIntersectionTacticalPlugin * * \return List of centerline points paired with target speeds */ - std::vector create_case_one_speed_profile(const carma_wm::WorldModelConstPtr& wm, const cav_msgs::Maneuver& maneuver, - std::vector& route_geometry_points, double starting_speed, const cav_msgs::VehicleState& states); + std::vector create_case_one_speed_profile(const carma_wm::WorldModelConstPtr& wm, const carma_planning_msgs::msg::Maneuver& maneuver, + std::vector& route_geometry_points, double starting_speed, const carma_planning_msgs::msg::VehicleState& states); /** * \brief Creates a speed profile according to case two of the stop controlled intersection, @@ -128,7 +109,7 @@ class StopControlledIntersectionTacticalPlugin * * \return List of centerline points paired with speed limits */ - std::vector create_case_two_speed_profile(const carma_wm::WorldModelConstPtr& wm, const cav_msgs::Maneuver& maneuver, + std::vector create_case_two_speed_profile(const carma_wm::WorldModelConstPtr& wm, const carma_planning_msgs::msg::Maneuver& maneuver, std::vector& route_geometry_points, double starting_speed); /** @@ -144,7 +125,7 @@ class StopControlledIntersectionTacticalPlugin * * \return List of centerline points paired with speed limits */ - std::vector create_case_three_speed_profile(const carma_wm::WorldModelConstPtr& wm, const cav_msgs::Maneuver& maneuver, + std::vector create_case_three_speed_profile(const carma_wm::WorldModelConstPtr& wm, const carma_planning_msgs::msg::Maneuver& maneuver, std::vector& route_geometry_points, double starting_speed); /** @@ -157,30 +138,34 @@ class StopControlledIntersectionTacticalPlugin * * \return A list of trajectory points to send to the carma planning stack */ - std::vector compose_trajectory_from_centerline( - const std::vector& points, const cav_msgs::VehicleState& state, const ros::Time& state_time); - - - /** - * \brief Method to call at fixed rate in execution loop. Will publish plugin discovery updates - * - * \return True if the node should continue running. False otherwise - */ - bool onSpin(); + std::vector compose_trajectory_from_centerline( + const std::vector& points, const carma_planning_msgs::msg::VehicleState& state, const rclcpp::Time& state_time); + + // overrides + carma_ros2_utils::CallbackReturn on_configure_plugin() override; + bool get_availability(); + std::string get_version_id(); + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + void plan_trajectory_callback( + std::shared_ptr, + carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, + carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override; private: carma_wm::WorldModelConstPtr wm_; StopControlledIntersectionTacticalPluginConfig config_; - PublishPluginDiscoveryCB plugin_discovery_publisher_; - - cav_msgs::Plugin plugin_discovery_msg_; - carma_debug_msgs::TrajectoryCurvatureSpeeds debug_msg_; std::string stop_controlled_intersection_strategy_ = "Carma/stop_controlled_intersection"; double epsilon_ = 0.001; //Small constant to compare (double) 0.0 with - + // Unit Test Accessors + FRIEND_TEST(StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_one); + FRIEND_TEST(StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_two); + FRIEND_TEST(StopControlledIntersectionTacticalPlugin, TestSCIPlanning_case_three); }; -}; \ No newline at end of file + + +} // namespace stop_controlled_intersection_tactical_plugin \ No newline at end of file diff --git a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h b/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h deleted file mode 100644 index 6764f8b8ee..0000000000 --- a/stop_controlled_intersection_tactical_plugin/include/stop_controlled_intersection_plugin_node.h +++ /dev/null @@ -1,98 +0,0 @@ -#pragma once - -/* - * Copyright (C) 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace stop_controlled_intersection_transit_plugin -{ - /** - * \brief ROS node for the StopControlledIntersectionTransitPlugin - */ -class StopControlledIntersectionTransitPluginNode -{ -public: - /** - * \brief Entrypoint for this node - */ - void run() - { - ros::CARMANodeHandle nh; - ros::CARMANodeHandle pnh("~"); - - carma_wm::WMListener wml; - auto wm_ = wml.getWorldModel(); - - StopControlledIntersectionTacticalPluginConfig config; - - ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); - - // // Determine if we will enable debug publishing by checking the current log level of the node - // std::map logger; - // ros::console::get_loggers(logger); - // config.publish_debug = logger[ROSCONSOLE_DEFAULT_NAME] == ros::console::levels::Debug; - - StopControlledIntersectionTacticalPlugin worker(wm_, config, [&discovery_pub](auto& msg) { discovery_pub.publish(msg); }); - // [trajectory_debug_pub](const auto& msg) { trajectory_debug_pub.publish(msg); }); - - ros::ServiceServer trajectory_srv_ = nh.advertiseService("stop_controlled_intersection_tactical_plugin/plan_trajectory", - &StopControlledIntersectionTacticalPlugin::plan_trajectory_cb, &worker); - - ros::Timer discovery_pub_timer_ = - pnh.createTimer(ros::Duration(ros::Rate(10.0)), [&worker](const auto&) { worker.onSpin(); }); - - ros::CARMANodeHandle::spin(); - } -}; - -} //namespace stop_controlled_intersection_plugin \ No newline at end of file diff --git a/stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch b/stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch deleted file mode 100644 index b721c967ff..0000000000 --- a/stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - diff --git a/stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch.py b/stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch.py new file mode 100644 index 0000000000..04792ff0d0 --- /dev/null +++ b/stop_controlled_intersection_tactical_plugin/launch/stop_controlled_intersection_tactical_plugin.launch.py @@ -0,0 +1,68 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + + +''' +This file is can be used to launch the CARMA stop_controlled_intersection_tactical_plugin_container. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + stop_controlled_intersection_tactical_plugin_file_path = os.path.join( + get_package_share_directory('stop_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') + + # Launch node(s) in a carma container to allow logging to be configured + container = ComposableNodeContainer( + package='carma_ros2_utils', + name='stop_controlled_intersection_tactical_plugin_container', + namespace=GetCurrentNamespace(), + executable='carma_component_container_mt', + composable_node_descriptions=[ + + # Launch the core node(s) + ComposableNode( + package='stop_controlled_intersection_tactical_plugin', + plugin='stop_controlled_intersection_tactical_plugin::StopControlledIntersectionTacticalPlugin', + name='stop_controlled_intersection_tactical_plugin', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ], + parameters=[ + stop_controlled_intersection_tactical_plugin_file_path + ] + ), + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + container + ]) diff --git a/stop_controlled_intersection_tactical_plugin/package.xml b/stop_controlled_intersection_tactical_plugin/package.xml index 44e5daabe8..a27a52a3e3 100644 --- a/stop_controlled_intersection_tactical_plugin/package.xml +++ b/stop_controlled_intersection_tactical_plugin/package.xml @@ -1,7 +1,7 @@ - - - - + - + diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index bd65c1d828..e7c03b600e 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -39,6 +39,7 @@ def generate_launch_description(): route_file_folder = LaunchConfiguration('route_file_folder') + vehicle_calibration_dir = LaunchConfiguration('vehicle_calibration_dir') vehicle_characteristics_param_file = LaunchConfiguration('vehicle_characteristics_param_file') enable_guidance_plugin_validator = LaunchConfiguration('enable_guidance_plugin_validator') strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate') @@ -247,7 +248,8 @@ def generate_launch_description(): parameters=[ trajectory_visualizer_param_file ] - ) + ) + ] ) @@ -259,6 +261,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/plugins.launch.py']), launch_arguments={ 'route_file_folder' : route_file_folder, + 'vehicle_calibration_dir' : vehicle_calibration_dir, 'vehicle_characteristics_param_file' : vehicle_characteristics_param_file, 'vehicle_config_param_file' : vehicle_config_param_file, 'enable_guidance_plugin_validator' : enable_guidance_plugin_validator, diff --git a/carma/launch/plugins.launch.py b/carma/launch/plugins.launch.py index be2e965cb3..b8875b7db6 100644 --- a/carma/launch/plugins.launch.py +++ b/carma/launch/plugins.launch.py @@ -36,6 +36,7 @@ def generate_launch_description(): route_file_folder = LaunchConfiguration('route_file_folder') + vehicle_calibration_dir = LaunchConfiguration('vehicle_calibration_dir') vehicle_characteristics_param_file = LaunchConfiguration('vehicle_characteristics_param_file') enable_guidance_plugin_validator = LaunchConfiguration('enable_guidance_plugin_validator') strategic_plugins_to_validate = LaunchConfiguration('strategic_plugins_to_validate') @@ -51,7 +52,7 @@ def generate_launch_description(): get_package_share_directory('route_following_plugin'), 'config/parameters.yaml') stop_and_wait_plugin_param_file = os.path.join( - get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') + get_package_share_directory('stop_and_wait_plugin'), 'config/parameters.yaml') light_controlled_intersection_tactical_plugin_param_file = os.path.join( get_package_share_directory('light_controlled_intersection_tactical_plugin'), 'config/parameters.yaml') @@ -76,6 +77,8 @@ def generate_launch_description(): env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') + pure_pursuit_tuning_parameters = [vehicle_calibration_dir, "/pure_pursuit/calibration.yaml"] + carma_plugins_container = ComposableNodeContainer( package='carma_ros2_utils', name='carma_guidance_core_plugins_container', @@ -270,73 +273,30 @@ def generate_launch_description(): stop_controlled_intersection_tactical_plugin_file_path, vehicle_config_param_file ] - ) - ] - ) - - platooning_plugins_container = ComposableNodeContainer( - package='carma_ros2_utils', - name='platooning_plugins_container', - executable='carma_component_container_mt', - namespace=GetCurrentNamespace(), - composable_node_descriptions=[ + ), ComposableNode( - package='platoon_strategic_ihp', - plugin='platoon_strategic_ihp::Node', - name='platoon_strategic_ihp_node', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('platoon_strategic_ihp', env_log_levels) } + package='pure_pursuit_wrapper', + plugin='pure_pursuit_wrapper::PurePursuitWrapperNode', + name='pure_pursuit_wrapper', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('pure_pursuit_wrapper', env_log_levels) } ], 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" ] ), - ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), - ("outgoing_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_response" ] ), - ("outgoing_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_request" ] ), - ("outgoing_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_operation" ] ), - ("incoming_mobility_request", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_request" ] ), - ("incoming_mobility_response", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_response" ] ), - ("incoming_mobility_operation", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_mobility_operation" ] ), - ("incoming_spat", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/incoming_spat" ] ), - ("twist_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/twist_raw" ] ), - ("platoon_info", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/platoon_info" ] ), ("plugin_discovery", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugin_discovery" ] ), - ("route", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/route" ] ), - ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), + ("ctrl_raw", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/ctrl_raw" ] ), + ("pure_pursuit_wrapper/plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plugins/pure_pursuit/plan_trajectory" ] ), ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ), + ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ), ], - parameters=[ - platoon_strategic_ihp_param_file, - vehicle_config_param_file + parameters=[ + vehicle_characteristics_param_file, #vehicle_response_lag + pure_pursuit_tuning_parameters #pure_pursuit calibration parameters ] - ), - ComposableNode( - package='platooning_tactical_plugin', - plugin='platooning_tactical_plugin::Node', - name='platooning_tactical_plugin_node', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('platooning_tactical_plugin', env_log_levels) } - ], - 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" ] ), - ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), - ("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" ] ), - ], - parameters=[ platoon_tactical_ihp_param_file, vehicle_config_param_file ] ), - ] ) - - platooning_plugins_container = ComposableNodeContainer( package='carma_ros2_utils', name='platooning_plugins_container', @@ -376,13 +336,13 @@ def generate_launch_description(): ] ), ComposableNode( - package='platooning_tactical_plugin', - plugin='platooning_tactical_plugin::Node', - name='platooning_tactical_plugin_node', - extra_arguments=[ - {'use_intra_process_comms': True}, - {'--log-level' : GetLogLevel('platooning_tactical_plugin', env_log_levels) } - ], + package='platooning_tactical_plugin', + plugin='platooning_tactical_plugin::Node', + name='platooning_tactical_plugin_node', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : GetLogLevel('platooning_tactical_plugin', env_log_levels) } + ], remappings = [ ("semantic_map", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/semantic_map" ] ), ("map_update", [ EnvironmentVariable('CARMA_ENV_NS', default_value=''), "/map_update" ] ), @@ -394,12 +354,9 @@ def generate_launch_description(): ], parameters=[ platoon_tactical_ihp_param_file, vehicle_config_param_file ] ), - ] ) - - return LaunchDescription([ carma_plugins_container, platooning_plugins_container diff --git a/carma/rviz/carma_default.rviz b/carma/rviz/carma_default.rviz index ed88f9dacd..4956a3ed82 100644 --- a/carma/rviz/carma_default.rviz +++ b/carma/rviz/carma_default.rviz @@ -10,7 +10,7 @@ Panels: - /MobilityPath Collision Warning1 - /TCM Visualizer1 Splitter Ratio: 0.5446686148643494 - Tree Height: 391 + Tree Height: 865 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -134,7 +134,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -149,7 +149,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /vector_map @@ -214,7 +214,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -229,7 +229,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /detection_range @@ -662,7 +662,7 @@ Visualization Manager: Value: true Views: Current: - Angle: -1.159999966621399 + Angle: -0.32499998807907104 Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 @@ -674,8 +674,8 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Scale: 10 Target Frame: base_link - X: 15.907857894897461 - Y: -9.377481460571289 + X: 11.995138168334961 + Y: 17.10146141052246 Saved: ~ Window Geometry: Camera: @@ -687,7 +687,7 @@ Window Geometry: Hide Right Dock: true Image: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -697,5 +697,5 @@ Window Geometry: Views: collapsed: true Width: 915 - X: 302 - Y: 146 + X: 759 + Y: 262 diff --git a/carma_wm_ctrl/config/parameters.yaml b/carma_wm_ctrl/config/parameters.yaml index 9b923708b1..5d54aaf0af 100644 --- a/carma_wm_ctrl/config/parameters.yaml +++ b/carma_wm_ctrl/config/parameters.yaml @@ -10,4 +10,4 @@ traffic_control_request_period: 3.0 intersection_ids_for_correction: [9945, 9709] #List of Double: Every 2 element describes coordinate correction [delta_x, delta_y] for each intersection_id in intersection_ids_for_correction in same order -intersection_coord_correction: [2.5, -2.5, -0.5, -1.0] +intersection_coord_correction: [2.5, -2.5, -0.5, -1.0] \ No newline at end of file diff --git a/pure_pursuit_wrapper/CMakeLists.txt b/pure_pursuit_wrapper/CMakeLists.txt index 59075c9bf0..e31fa40ba1 100644 --- a/pure_pursuit_wrapper/CMakeLists.txt +++ b/pure_pursuit_wrapper/CMakeLists.txt @@ -1,5 +1,5 @@ -# Copyright (C) 2019-2021 LEIDOS. +# 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 @@ -13,224 +13,66 @@ # License for the specific language governing permissions and limitations under # the License. -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(pure_pursuit_wrapper) find_package(carma_cmake_common REQUIRED) -carma_check_ros_version(1) +carma_check_ros_version(2) carma_package() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -set( CATKIN_DEPS - cav_msgs - roscpp - rospy - std_msgs - geometry_msgs - autoware_msgs - autoware_config_msgs - message_filters - carma_utils - trajectory_utils - carma_wm -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_DEPS} -) - -## System dependencies are found with CMake's conventions -find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# cav_msgs# std_msgs -# ) - -add_library(${PROJECT_NAME} src/pure_pursuit_wrapper.cpp) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS ${CATKIN_DEPS} - DEPENDS Boost -) - -########### -## Build ## -########### +## Find dependencies using ament auto +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() ## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/pure_pursuit_wrapper.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node src/pure_pursuit_wrapper_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries( ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +# Name build targets +set(node_lib pure_pursuit_wrapper_node_lib) +set(node_exec pure_pursuit_wrapper_node_exec) -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME} -) -############# -## Install ## -############# +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -# Mark executable scripts (Python etc.) for installation -# in contrast to setup.py, you can choose the destination -install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# Build +ament_auto_add_library(${node_lib} SHARED + src/pure_pursuit_wrapper.cpp ) - -# Mark executables for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h" - PATTERN ".svn" EXCLUDE +ament_auto_add_executable(${node_exec} + src/main.cpp ) -# Mark other files for installation (e.g. launch and bag files, etc.) -install(DIRECTORY - launch - config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Register component +rclcpp_components_register_nodes(${node_lib} "pure_pursuit_wrapper::PurePursuitWrapperNode") + +target_link_libraries(${node_exec} + ${node_lib} ) -############# -## Testing ## -############# +# Testing +if(BUILD_TESTING) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable + + ament_add_gtest(${PROJECT_NAME}-test + test/sanity_checks.cpp + #test/test_pure_pursuit.cpp + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test # Add test directory as working directory for unit tests + ) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_pure_pursuit_wrapper.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() + ament_target_dependencies(${PROJECT_NAME}-test ${${PROJECT_NAME}_FOUND_TEST_DEPENDS}) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) + target_link_libraries(${PROJECT_NAME}-test ${node_lib}) -catkin_add_gtest(${PROJECT_NAME}_test test/${PROJECT_NAME}_test.cpp) +endif() -target_link_libraries( ${PROJECT_NAME}_test ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) +# Install +ament_auto_package( + INSTALL_TO_SHARE config launch +) \ No newline at end of file diff --git a/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper.hpp b/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper.hpp index c14a54c6bf..b6f777321a 100644 --- a/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper.hpp +++ b/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper.hpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-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 @@ -16,55 +16,76 @@ #include // ROS -#include -#include +#include // msgs -#include -#include +#include +#include #include "pure_pursuit_wrapper_config.hpp" #include -#include +#include +#include +#include +#include +#include namespace pure_pursuit_wrapper { -using WaypointPub = std::function; -using PluginDiscoveryPub = std::function; -/*! - * Main class for the node to handle the ROS interfacing. - */ +using WaypointPub = std::function; +using PluginDiscoveryPub = std::function; +namespace pure_pursuit = autoware::motion::control::pure_pursuit; -class PurePursuitWrapper { +class PurePursuitWrapperNode : public carma_guidance_plugins::ControlPlugin +{ public: + /** + * @brief Constructor + */ + explicit PurePursuitWrapperNode(const rclcpp::NodeOptions& options); + + autoware_msgs::msg::ControlCommandStamped generate_command() override; + + /** + * \brief Example callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + + /** + * \brief This method should be used to load parameters and will be called on the configure state transition. + */ + carma_ros2_utils::CallbackReturn on_configure_plugin() override; + + bool get_availability() override; + + std::string get_version_id() override; + + /** + * \brief Drops any points that sequentially have same target_time and return new trajectory_points in order to avoid divide by zero situation + * \param traj_points Velocity profile to shift. The first point should be the current vehicle speed + * + * NOTE: This function assumes the target_time will not go backwards. In other words, it only removes "sequential" points that have same target_time + * \return A new trajectory without any repeated time_stamps + */ + std::vector remove_repeated_timestamps(const std::vector& traj_points); + + //CONVERSIONS + + motion::motion_common::State convert_state(geometry_msgs::msg::PoseStamped pose, geometry_msgs::msg::TwistStamped twist); + autoware_msgs::msg::ControlCommandStamped convert_cmd(motion::motion_common::Command cmd); - PurePursuitWrapper(PurePursuitWrapperConfig config, WaypointPub waypoint_pub, PluginDiscoveryPub plugin_discovery_pub); - - void trajectoryPlanHandler(const cav_msgs::TrajectoryPlan::ConstPtr& tp); - - bool onSpin(); - - /** - * \brief Applies a specified response lag in seconds to the trajectory shifting the whole thing by the specified lag time - * \param speeds Velocity profile to shift. The first point should be the current vehicle speed - * \param downtrack Distance points for each velocity point. Should have the same size as speeds and start from 0 - * \param response_lag The lag in seconds before which the vehicle will not meaningfully accelerate - * - * \return A Shifted trajectory - */ - std::vector apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag) const; - - /** - * \brief Drops any points that sequentially have same target_time and return new trajectory_points in order to avoid divide by zero situation - * \param traj_points Velocity profile to shift. The first point should be the current vehicle speed - * - * NOTE: This function assumes the target_time will not go backwards. In other words, it only removes "sequential" points that have same target_time - * \return A new trajectory without any repeated time_stamps - */ - std::vector remove_repeated_timestamps(const std::vector& traj_points); private: + PurePursuitWrapperConfig config_; - WaypointPub waypoint_pub_; - PluginDiscoveryPub plugin_discovery_pub_; - cav_msgs::Plugin plugin_discovery_msg_; + + std::shared_ptr pp_; + + std::shared_ptr get_pure_pursuit_worker() + { + return pp_; + } + + // Unit Test Accessors + FRIEND_TEST(PurePursuitTest, sanity_check); }; diff --git a/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper_config.hpp b/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper_config.hpp index fe8be086c8..6b0bb4abff 100644 --- a/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper_config.hpp +++ b/pure_pursuit_wrapper/include/pure_pursuit_wrapper/pure_pursuit_wrapper_config.hpp @@ -1,7 +1,7 @@ #pragma once /* - * Copyright (C) 2020 LEIDOS. + * 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 @@ -20,16 +20,45 @@ namespace pure_pursuit_wrapper { /** - * \brief Stuct containing the algorithm configuration values for the PurePursuitWrapperConfig + * \brief Struct containing the algorithm configuration values for the PurePursuitWrapperConfig */ struct PurePursuitWrapperConfig { double vehicle_response_lag = 0.2; // An approximation of the delay (sec) between sent vehicle commands and the vehicle begining a meaningful acceleration to that command + double minimum_lookahead_distance = 6.0; + double maximum_lookahead_distance = 100.0; + double speed_to_lookahead_ratio = 2.0; + bool is_interpolate_lookahead_point = true; + bool is_delay_compensation = true; // not to be confused with vehicle_response_lag, which is applied nonetheless + double emergency_stop_distance = 0.1; + double speed_thres_traveling_direction = 0.3; + double dist_front_rear_wheels = 3.5; + // integrator part + double dt = 0.1; + double integrator_max_pp = 0.0; + double integrator_min_pp = 0.0; + double Ki_pp = 0.0; + bool is_integrator_enabled = false; + friend std::ostream& operator<<(std::ostream& output, const PurePursuitWrapperConfig& c) { output << "PurePursuitWrapperConfig { " << std::endl << "vehicle_response_lag: " << c.vehicle_response_lag << std::endl + << "minimum_lookahead_distance: " << c.minimum_lookahead_distance << std::endl + << "maximum_lookahead_distance: " << c.maximum_lookahead_distance << std::endl + << "speed_to_lookahead_ratio: " << c.speed_to_lookahead_ratio << std::endl + << "is_interpolate_lookahead_point: " << c.is_interpolate_lookahead_point << std::endl + << "is_delay_compensation: " << c.is_delay_compensation << std::endl + << "emergency_stop_distance: " << c.emergency_stop_distance << std::endl + << "speed_thres_traveling_direction: " << c.speed_thres_traveling_direction << std::endl + << "dist_front_rear_wheels: " << c.dist_front_rear_wheels << std::endl + // integrator part: + << "dt: " << c.dt << std::endl + << "integrator_max_pp: " << c.integrator_max_pp << std::endl + << "integrator_min_pp: " << c.integrator_min_pp << std::endl + << "Ki_pp: " << c.Ki_pp << std::endl + << "is_integrator_enabled: " << c.is_integrator_enabled << std::endl << "}" << std::endl; return output; } diff --git a/pure_pursuit_wrapper/launch/pure_pursuit_wrapper.launch b/pure_pursuit_wrapper/launch/pure_pursuit_wrapper.launch deleted file mode 100644 index 4c95e320f1..0000000000 --- a/pure_pursuit_wrapper/launch/pure_pursuit_wrapper.launch +++ /dev/null @@ -1,33 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/pure_pursuit_wrapper/launch/pure_pursuit_wrapper.launch.py b/pure_pursuit_wrapper/launch/pure_pursuit_wrapper.launch.py new file mode 100644 index 0000000000..7e5219ea3c --- /dev/null +++ b/pure_pursuit_wrapper/launch/pure_pursuit_wrapper.launch.py @@ -0,0 +1,58 @@ +# 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. + +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from carma_ros2_utils.launch.get_current_namespace import GetCurrentNamespace + +import os + +''' +This file is can be used to launch the CARMA Pure Pursuit Wrapper Node. + Though in carma-platform it may be launched directly from the base launch file. +''' + +def generate_launch_description(): + + # Declare the log_level launch argument + log_level = LaunchConfiguration('log_level') + declare_log_level_arg = DeclareLaunchArgument( + name ='log_level', default_value='WARN') + + carma_pure_pursuit_wrapper_container = ComposableNodeContainer( + package='carma_ros2_utils', + name='carma_pure_pursuit_wrapper_container', + executable='carma_component_container_mt', + namespace=GetCurrentNamespace(), + composable_node_descriptions=[ + ComposableNode( + package='pure_pursuit_wrapper', + plugin='pure_pursuit_wrapper::PurePursuitWrapperNode', + name='pure_pursuit_wrapper', + extra_arguments=[ + {'use_intra_process_comms': True}, + {'--log-level' : log_level } + ] + ) + ] + ) + + return LaunchDescription([ + declare_log_level_arg, + carma_pure_pursuit_wrapper_container + ]) diff --git a/pure_pursuit_wrapper/package.xml b/pure_pursuit_wrapper/package.xml index f0ee87994e..3c62192cc7 100644 --- a/pure_pursuit_wrapper/package.xml +++ b/pure_pursuit_wrapper/package.xml @@ -1,7 +1,7 @@ pure_pursuit_wrapper - 3.3.0 + 4.0.0 The pure_pursuit_wrapper package @@ -9,7 +9,6 @@ carma - @@ -17,23 +16,33 @@ carma - catkin - cav_msgs - roscpp - rospy + ament_cmake + carma_cmake_common + ament_auto_cmake + + rclcpp + carma_ros2_utils + rclcpp_components + + carma_planning_msgs std_msgs autoware_msgs autoware_config_msgs geometry_msgs - carma_utils - trajectory_utils - carma_wm - message_filters - carma_cmake_common + trajectory_utils_ros2 + carma_wm_ros2 + carma_guidance_plugins + pure_pursuit + motion_testing + basic_autonomy_ros2 - - - + ament_lint_auto + ament_cmake_gtest + launch + launch_ros + + + ament_cmake diff --git a/pure_pursuit_wrapper/src/main.cpp b/pure_pursuit_wrapper/src/main.cpp new file mode 100644 index 0000000000..d5d4e3e002 --- /dev/null +++ b/pure_pursuit_wrapper/src/main.cpp @@ -0,0 +1,31 @@ +/* + * Copyright (C) 2018-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 + +// Main execution +int main(int argc, char** argv) +{ + // Initialize node + 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(); +}; \ No newline at end of file diff --git a/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp b/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp index 7d43f8bccd..5bd6f2e684 100644 --- a/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp +++ b/pure_pursuit_wrapper/src/pure_pursuit_wrapper.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018-2021 LEIDOS. + * Copyright (C) 2018-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 @@ -14,128 +14,192 @@ * the License. */ -#include "pure_pursuit_wrapper/pure_pursuit_wrapper.hpp" -#include -#include +#include +#include +#include #include namespace pure_pursuit_wrapper { -PurePursuitWrapper::PurePursuitWrapper(PurePursuitWrapperConfig config, WaypointPub waypoint_pub, PluginDiscoveryPub plugin_discovery_pub) - : config_(config), waypoint_pub_(waypoint_pub), plugin_discovery_pub_(plugin_discovery_pub) +namespace std_ph = std::placeholders; + +PurePursuitWrapperNode::PurePursuitWrapperNode(const rclcpp::NodeOptions& options) + : carma_guidance_plugins::ControlPlugin(options) { - plugin_discovery_msg_.name = "pure_pursuit_wrapper_node"; - plugin_discovery_msg_.version_id = "v1.0"; - plugin_discovery_msg_.available = true; - plugin_discovery_msg_.activated = true; - plugin_discovery_msg_.type = cav_msgs::Plugin::CONTROL; - plugin_discovery_msg_.capability = "control/trajectory_control"; + config_ = PurePursuitWrapperConfig(); + config_.vehicle_response_lag = declare_parameter("vehicle_response_lag", config_.vehicle_response_lag); + config_.minimum_lookahead_distance = declare_parameter("minimum_lookahead_distance", config_.minimum_lookahead_distance); + config_.maximum_lookahead_distance = declare_parameter("maximum_lookahead_distance", config_.maximum_lookahead_distance); + config_.speed_to_lookahead_ratio = declare_parameter("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio); + config_.is_interpolate_lookahead_point = declare_parameter("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point); + config_.is_delay_compensation = declare_parameter("is_delay_compensation", config_.is_delay_compensation); + config_.emergency_stop_distance = declare_parameter("emergency_stop_distance", config_.emergency_stop_distance); + config_.speed_thres_traveling_direction = declare_parameter("speed_thres_traveling_direction", config_.speed_thres_traveling_direction); + config_.dist_front_rear_wheels = declare_parameter("dist_front_rear_wheels", config_.dist_front_rear_wheels); + + // integrator part + config_.dt = declare_parameter("dt", config_.dt); + config_.integrator_max_pp = declare_parameter("integrator_max_pp", config_.integrator_max_pp); + config_.integrator_min_pp = declare_parameter("integrator_min_pp", config_.integrator_min_pp); + config_.Ki_pp = declare_parameter("Ki_pp", config_.Ki_pp); + config_.is_integrator_enabled = declare_parameter("is_integrator_enabled", config_.is_integrator_enabled); } -bool PurePursuitWrapper::onSpin() +carma_ros2_utils::CallbackReturn PurePursuitWrapperNode::on_configure_plugin() { - plugin_discovery_pub_(plugin_discovery_msg_); - return true; -} + config_ = PurePursuitWrapperConfig(); + get_parameter("vehicle_response_lag", config_.vehicle_response_lag); + get_parameter("minimum_lookahead_distance", config_.minimum_lookahead_distance); + get_parameter("maximum_lookahead_distance", config_.maximum_lookahead_distance); + get_parameter("speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio); + get_parameter("is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point); + get_parameter("is_delay_compensation", config_.is_delay_compensation); + get_parameter("emergency_stop_distance", config_.emergency_stop_distance); + get_parameter("speed_thres_traveling_direction", config_.speed_thres_traveling_direction); + get_parameter("dist_front_rear_wheels", config_.dist_front_rear_wheels); + + // integrator configs + get_parameter("dt", config_.dt); + get_parameter("integrator_max_pp", config_.integrator_max_pp); + get_parameter("integrator_min_pp", config_.integrator_min_pp); + get_parameter("Ki_pp", config_.Ki_pp); + get_parameter("is_integrator_enabled", config_.is_integrator_enabled); + RCLCPP_INFO_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "Loaded Params: " << config_); -void PurePursuitWrapper::trajectoryPlanHandler(const cav_msgs::TrajectoryPlan::ConstPtr& tp) + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&PurePursuitWrapperNode::parameter_update_callback, this, std_ph::_1)); + + // create config for pure_pursuit worker + pure_pursuit::Config cfg{ + config_.minimum_lookahead_distance, + config_.maximum_lookahead_distance, + config_.speed_to_lookahead_ratio, + config_.is_interpolate_lookahead_point, + config_.is_delay_compensation, + config_.emergency_stop_distance, + config_.speed_thres_traveling_direction, + config_.dist_front_rear_wheels, + }; + + pure_pursuit::IntegratorConfig i_cfg; + i_cfg.dt = config_.dt; + i_cfg.integrator_max_pp = config_.integrator_max_pp; + i_cfg.integrator_min_pp = config_.integrator_min_pp; + i_cfg.Ki_pp = config_.Ki_pp; + i_cfg.integral = 0.0; // accumulator of integral starts from 0 + i_cfg.is_integrator_enabled = config_.is_integrator_enabled; + + pp_ = std::make_shared(cfg, i_cfg); + + // Return success if everything initialized successfully + return CallbackReturn::SUCCESS; +} + +motion::motion_common::State PurePursuitWrapperNode::convert_state(geometry_msgs::msg::PoseStamped pose, geometry_msgs::msg::TwistStamped twist) { - ROS_DEBUG_STREAM("Received TrajectoryPlanCurrentPosecallback message"); + motion::motion_common::State state; + state.header = pose.header; + state.state.x = pose.pose.position.x; + state.state.y = pose.pose.position.y; + state.state.z = pose.pose.position.z; + state.state.heading.real = pose.pose.orientation.w; + state.state.heading.imag = pose.pose.orientation.z; + + state.state.longitudinal_velocity_mps = twist.twist.linear.x; + return state; +} - std::vector times; - std::vector downtracks; +autoware_msgs::msg::ControlCommandStamped PurePursuitWrapperNode::convert_cmd(motion::motion_common::Command cmd) +{ + autoware_msgs::msg::ControlCommandStamped return_cmd; + return_cmd.header.stamp = cmd.stamp; - std::vector trajectory_points = tp->trajectory_points; + return_cmd.cmd.linear_acceleration = cmd.long_accel_mps2; + return_cmd.cmd.linear_velocity = cmd.velocity_mps; + return_cmd.cmd.steering_angle = cmd.front_wheel_angle_rad; - ROS_DEBUG_STREAM("Original Trajectory size:"< speeds; - trajectory_utils::conversions::time_to_speed(downtracks, times, tp->initial_longitudinal_velocity, &speeds); + motion::control::controller_common::State state_tf = convert_state(current_pose_.get(), current_twist_.get()); - if (speeds.size() != trajectory_points.size()) - { - throw std::invalid_argument("Speeds and trajectory points sizes do not match"); - } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "Forced from frame_id: " << state_tf.header.frame_id << ", into: " << current_trajectory_.get().header.frame_id); - for (size_t i = 0; i < speeds.size(); i++) { // Ensure 0 is min speed - if (stopping_index != 0 && i >= stopping_index - 1) - { - speeds[i] = 0.0; //stopping case - } - else - { - speeds[i] = std::max(0.0, speeds[i]); - } - } + current_trajectory_.get().header.frame_id = state_tf.header.frame_id; - std::vector lag_speeds = apply_response_lag(speeds, downtracks, config_.vehicle_response_lag); // This call requires that the first speed point be current speed to work as expected + auto autoware_traj_plan = basic_autonomy::waypoint_generation::process_trajectory_plan(current_trajectory_.get(), config_.vehicle_response_lag); - autoware_msgs::Lane lane; - lane.header = tp->header; - std::vector waypoints; - waypoints.reserve(trajectory_points.size()); + pp_->set_trajectory(autoware_traj_plan); - for (int i = 0; i < trajectory_points.size(); i++) - { - autoware_msgs::Waypoint wp; + const auto cmd{pp_->compute_command(state_tf)}; + + converted_cmd = convert_cmd(cmd); - wp.pose.pose.position.x = trajectory_points[i].x; - wp.pose.pose.position.y = trajectory_points[i].y; - wp.twist.twist.linear.x = lag_speeds[i]; - ROS_DEBUG_STREAM("Setting waypoint idx: " << i <<", with planner: << " << trajectory_points[i].planner_plugin_name << ", x: " << trajectory_points[i].x << - ", y: " << trajectory_points[i].y << - ", speed: " << lag_speeds[i]* 2.23694 << "mph"); - waypoints.push_back(wp); - } - lane.waypoints = waypoints; + return converted_cmd; +} - waypoint_pub_(lane); +rcl_interfaces::msg::SetParametersResult PurePursuitWrapperNode::parameter_update_callback(const std::vector ¶meters) +{ + auto error_double = update_params({ + {"vehicle_response_lag", config_.vehicle_response_lag}, + {"minimum_lookahead_distance", config_.minimum_lookahead_distance}, + {"maximum_lookahead_distance", config_.maximum_lookahead_distance}, + {"speed_to_lookahead_ratio", config_.speed_to_lookahead_ratio}, + {"emergency_stop_distance", config_.emergency_stop_distance}, + {"speed_thres_traveling_direction", config_.speed_thres_traveling_direction}, + {"dist_front_rear_wheels", config_.dist_front_rear_wheels}, + {"integrator_max_pp", config_.integrator_max_pp}, + {"integrator_min_pp", config_.integrator_min_pp}, + {"Ki_pp", config_.Ki_pp} + }, parameters); + + auto error_bool = update_params({ + {"is_interpolate_lookahead_point", config_.is_interpolate_lookahead_point}, + {"is_delay_compensation", config_.is_delay_compensation}, + {"is_integrator_enabled", config_.is_integrator_enabled} + }, parameters); -}; + rcl_interfaces::msg::SetParametersResult result; + result.successful = !error_double && !error_bool; -std::vector PurePursuitWrapper::apply_response_lag(const std::vector& speeds, const std::vector downtracks, double response_lag) const { // Note first speed is assumed to be vehicle speed - if (speeds.size() != downtracks.size()) { - throw std::invalid_argument("Speed list and downtrack list are not the same size."); - } + return result; +} - std::vector output; - if (speeds.empty()) { - return output; - } - double lookahead_distance = speeds[0] * response_lag; +bool PurePursuitWrapperNode::get_availability() +{ + return true; +} - double downtrack_cutoff = downtracks[0] + lookahead_distance; - size_t lookahead_count = std::lower_bound(downtracks.begin(),downtracks.end(), downtrack_cutoff) - downtracks.begin(); // Use binary search to find lower bound cutoff point - output = trajectory_utils::shift_by_lookahead(speeds, (unsigned int) lookahead_count); - return output; +std::string PurePursuitWrapperNode::get_version_id() +{ + return "v4.0"; } -std::vector PurePursuitWrapper::remove_repeated_timestamps(const std::vector& traj_points){ +std::vector PurePursuitWrapperNode::remove_repeated_timestamps(const std::vector& traj_points) +{ - std::vector new_traj_points; + std::vector new_traj_points; - cav_msgs::TrajectoryPlanPoint prev_point; + carma_planning_msgs::msg::TrajectoryPlanPoint prev_point; bool first = true; for(auto point : traj_points){ @@ -152,7 +216,7 @@ std::vector PurePursuitWrapper::remove_repeated_t prev_point = point; } else{ - ROS_DEBUG_STREAM("Duplicate point found"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("pure_pursuit_wrapper"), "Duplicate point found"); } } @@ -161,3 +225,8 @@ std::vector PurePursuitWrapper::remove_repeated_t } } // namespace pure_pursuit_wrapper + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader +RCLCPP_COMPONENTS_REGISTER_NODE(pure_pursuit_wrapper::PurePursuitWrapperNode) diff --git a/pure_pursuit_wrapper/src/pure_pursuit_wrapper_node.cpp b/pure_pursuit_wrapper/src/pure_pursuit_wrapper_node.cpp deleted file mode 100644 index a662fd71d7..0000000000 --- a/pure_pursuit_wrapper/src/pure_pursuit_wrapper_node.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * 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. - */ -#include "pure_pursuit_wrapper/pure_pursuit_wrapper.hpp" -#include "pure_pursuit_wrapper/pure_pursuit_wrapper_config.hpp" - -#include -#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "pure_pursuit_wrapper_node"); - ros::CARMANodeHandle nh; - - ros::Publisher waypoints_pub = nh.advertise("final_waypoints", 10, true); - - ros::Publisher discovery_pub = nh.advertise("plugin_discovery", 1); - - pure_pursuit_wrapper::PurePursuitWrapperConfig config; - nh.param("/vehicle_response_lag", config.vehicle_response_lag, config.vehicle_response_lag); - - - pure_pursuit_wrapper::PurePursuitWrapper purePursuitWrapper( - config, - [&waypoints_pub](auto msg) { waypoints_pub.publish(msg); }, - [&discovery_pub](auto msg) { discovery_pub.publish(msg); }); - - // Trajectory Plan Subscriber - ros::Subscriber trajectory_plan_sub = nh.subscribe( - "pure_pursuit/plan_trajectory", 1, &pure_pursuit_wrapper::PurePursuitWrapper::trajectoryPlanHandler, &purePursuitWrapper); - - ros::Timer discovery_pub_timer_ = nh.createTimer( - ros::Duration(ros::Rate(10.0)), - [&purePursuitWrapper](const auto&) { purePursuitWrapper.onSpin(); }); - - ROS_INFO("Successfully launched node."); - ros::CARMANodeHandle::spin(); - - return 0; -} \ No newline at end of file diff --git a/pure_pursuit_wrapper/test/pure_pursuit_wrapper_test.cpp b/pure_pursuit_wrapper/test/pure_pursuit_wrapper_test.cpp deleted file mode 100755 index c481ffe22f..0000000000 --- a/pure_pursuit_wrapper/test/pure_pursuit_wrapper_test.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * 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. - */ - -#include "pure_pursuit_wrapper/pure_pursuit_wrapper.hpp" - -#include -#include -#include - -TEST(pure_pursuit_wrapper, trajectoryPlanHandler) -{ - cav_msgs::TrajectoryPlan plan; - plan.initial_longitudinal_velocity = 8.5; - - boost::optional wp_msg; - boost::optional plugin_msg; - pure_pursuit_wrapper::PurePursuitWrapperConfig config; - pure_pursuit_wrapper::PurePursuitWrapper ppw(config, [&wp_msg](auto msg) { wp_msg = msg; }, - [&plugin_msg](auto msg) { plugin_msg = msg; }); - - cav_msgs::TrajectoryPlanPoint tpp, tpp2, tpp3; - tpp.x = 10; - tpp.y = 10; - tpp.target_time = ros::Time(0.1); // 8.5m/s - - tpp2.x = 12; - tpp2.y = 12; - tpp2.target_time = ros::Time(0.2); // 48.068542495 m/s - - tpp3.x = 14; - tpp3.y = 14; - tpp3.target_time = ros::Time(0.3); // 8.5m/s - - plan.trajectory_points = { tpp, tpp2, tpp3 }; - - cav_msgs::TrajectoryPlan::ConstPtr plan_ptr(new cav_msgs::TrajectoryPlan(plan)); - - ASSERT_FALSE(!!wp_msg); - - ppw.trajectoryPlanHandler(plan_ptr); - - ASSERT_TRUE(!!wp_msg); - - autoware_msgs::Lane lane = wp_msg.get(); - - ASSERT_EQ(3, lane.waypoints.size()); - ASSERT_NEAR(8.5, lane.waypoints[0].twist.twist.linear.x, 0.0000001); - ASSERT_NEAR(10.0, lane.waypoints[0].pose.pose.position.x, 0.0000001); - ASSERT_NEAR(10.0, lane.waypoints[0].pose.pose.position.y, 0.0000001); - - ASSERT_NEAR(48.068542495, lane.waypoints[1].twist.twist.linear.x, 0.0000001); - ASSERT_NEAR(12.0, lane.waypoints[1].pose.pose.position.x, 0.0000001); - ASSERT_NEAR(12.0, lane.waypoints[1].pose.pose.position.y, 0.0000001); - - ASSERT_NEAR(8.5, lane.waypoints[2].twist.twist.linear.x, 0.0000001); - ASSERT_NEAR(14.0, lane.waypoints[2].pose.pose.position.x, 0.0000001); - ASSERT_NEAR(14.0, lane.waypoints[2].pose.pose.position.y, 0.0000001); -} - -TEST(pure_pursuit_wrapper, onSpin) -{ - cav_msgs::TrajectoryPlan plan; - plan.initial_longitudinal_velocity = 8.5; - - boost::optional wp_msg; - boost::optional plugin_msg; - pure_pursuit_wrapper::PurePursuitWrapperConfig config; - pure_pursuit_wrapper::PurePursuitWrapper ppw(config, [&wp_msg](auto msg) { wp_msg = msg; }, - [&plugin_msg](auto msg) { plugin_msg = msg; }); - - ASSERT_FALSE(!!plugin_msg); - - ASSERT_TRUE(ppw.onSpin()); - - ASSERT_TRUE(!!plugin_msg); - - cav_msgs::Plugin msg = plugin_msg.get(); - ASSERT_EQ(0, msg.name.compare("pure_pursuit_wrapper_node")); - ASSERT_EQ(0, msg.version_id.compare("v1.0")); - ASSERT_TRUE(msg.available); - ASSERT_TRUE(msg.activated); - ASSERT_EQ(cav_msgs::Plugin::CONTROL, msg.type); - ASSERT_EQ(0, msg.capability.compare("control/trajectory_control")); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/pure_pursuit_wrapper/test/sanity_checks.cpp b/pure_pursuit_wrapper/test/sanity_checks.cpp new file mode 100644 index 0000000000..dd40faa836 --- /dev/null +++ b/pure_pursuit_wrapper/test/sanity_checks.cpp @@ -0,0 +1,102 @@ +/* + * 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. + */ +#include +#include +#include +#include +#include +#include + +#include + +using motion::motion_testing::constant_velocity_trajectory; +using motion::motion_testing::make_state; +using autoware::motion::control::pure_pursuit::Config; +using autoware::motion::control::pure_pursuit::PurePursuit; +using autoware::motion::control::pure_pursuit::VehicleControlCommand; +using autoware::common::types::bool8_t; +using autoware::common::types::float32_t; + +namespace pure_pursuit_wrapper { + +TEST(PurePursuitTest, sanity_check) +{ + auto node = std::make_shared(rclcpp::NodeOptions()); + node->configure(); + node->activate(); + + carma_planning_msgs::msg::TrajectoryPlanPoint tpp, tpp2, tpp3; + tpp.x = 100; + tpp.y = 100; + tpp.target_time = rclcpp::Time(1.0*1e9); // 14.14 m/s + + tpp2.x = 110; + tpp2.y = 110; + tpp2.target_time = rclcpp::Time(2.0*1e9); // 14.14 m/s + + tpp3.x = 120; + tpp3.y = 120; + tpp3.target_time = rclcpp::Time(3.0*1e9); // 14.14 m/s + + carma_planning_msgs::msg::TrajectoryPlan plan; + plan.initial_longitudinal_velocity = 14.14; + + motion::control::controller_common::State state_tf; + auto converted_time_now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + + state_tf.header.stamp = rclcpp::Time(converted_time_now*1e9); + + state_tf.state.heading.real = 3.14 / 2; + state_tf.state.heading.imag = 1.0; + + state_tf.state.x = 0; + state_tf.state.y = 0; + state_tf.state.longitudinal_velocity_mps = 4.0; //arbitrary speed for first point + plan.header.frame_id = state_tf.header.frame_id; + plan.header.stamp = rclcpp::Time(converted_time_now*1e9) + rclcpp::Duration(1.0*1e9); + + plan.trajectory_points = { tpp, tpp2, tpp3 }; + + auto traj = basic_autonomy::waypoint_generation::process_trajectory_plan(plan, 0.0); + node->pp_->set_trajectory(traj); + + const auto cmd{node->get_pure_pursuit_worker()->compute_command(state_tf)}; + + ASSERT_NEAR(cmd.front_wheel_angle_rad, -0.294355, 0.005); + ASSERT_NEAR(cmd.long_accel_mps2, 0.311803, 0.005); + ASSERT_NEAR(cmd.rear_wheel_angle_rad, 0, 0.001); + ASSERT_NEAR(cmd.velocity_mps, 14.14, 0.01); + + auto converted_cmd = node->convert_cmd(cmd); + + ASSERT_NEAR(converted_cmd.cmd.linear_acceleration, cmd.long_accel_mps2, 0.001); + ASSERT_NEAR(converted_cmd.cmd.linear_velocity, cmd.velocity_mps, 0.01); + ASSERT_NEAR(converted_cmd.cmd.steering_angle, cmd.front_wheel_angle_rad, 0.001); +} +} // namespace pure_pursuit_wrapper + + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = 0; + try { + ret = RUN_ALL_TESTS(); + } catch (...) { + } + return ret; +} \ No newline at end of file diff --git a/route_following_plugin/launch/route_following_plugin_launch.py b/route_following_plugin/launch/route_following_plugin.launch.py similarity index 97% rename from route_following_plugin/launch/route_following_plugin_launch.py rename to route_following_plugin/launch/route_following_plugin.launch.py index 9436bddd06..2c81e85d70 100644 --- a/route_following_plugin/launch/route_following_plugin_launch.py +++ b/route_following_plugin/launch/route_following_plugin.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): ComposableNode( package='route_following_plugin', plugin='route_following_plugin::RouteFollowingPlugin', - name='route_following_plugin_node', + name='route_following_plugin', extra_arguments=[ {'use_intra_process_comms': True}, {'--log-level' : log_level } diff --git a/route_following_plugin/src/route_following_plugin.cpp b/route_following_plugin/src/route_following_plugin.cpp index 77a4101431..fa3ac718ca 100644 --- a/route_following_plugin/src/route_following_plugin.cpp +++ b/route_following_plugin/src/route_following_plugin.cpp @@ -14,6 +14,7 @@ * the License. */ #include +#include #include #include #include @@ -92,7 +93,7 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id config_.lane_change_plugin_= declare_parameter("lane_change_plugin", config_.lane_change_plugin_); config_.stop_and_wait_plugin_ = declare_parameter("stop_and_wait_plugin", config_.stop_and_wait_plugin_); config_.lanefollow_planning_tactical_plugin_ = declare_parameter("lane_following_plugin", config_.lanefollow_planning_tactical_plugin_); - config_.route_end_point_buffer_ = declare_parameter("/guidance/route/destination_downtrack_range", config_.route_end_point_buffer_); + config_.route_end_point_buffer_ = declare_parameter("guidance/route/destination_downtrack_range", config_.route_end_point_buffer_); config_.accel_limit_ = declare_parameter("vehicle_acceleration_limit", config_.accel_limit_); config_.lateral_accel_limit_ = declare_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit_); config_.stopping_accel_limit_multiplier_ = declare_parameter("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_); @@ -100,8 +101,21 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id config_.min_maneuver_length_ = declare_parameter("min_maneuver_length", config_.min_maneuver_length_); } - carma_ros2_utils::CallbackReturn RouteFollowingPlugin::on_configure_plugin() + carma_ros2_utils::CallbackReturn RouteFollowingPlugin::on_configure_plugin() { + config_ = Config(); + + get_parameter("minimal_plan_duration", config_.min_plan_duration_); + get_parameter("lane_change_plugin", config_.lane_change_plugin_); + get_parameter("stop_and_wait_plugin", config_.stop_and_wait_plugin_); + get_parameter("lane_following_plugin", config_.lanefollow_planning_tactical_plugin_); + get_parameter("guidance/route/destination_downtrack_range", config_.route_end_point_buffer_); + get_parameter("vehicle_acceleration_limit", config_.accel_limit_); + get_parameter("vehicle_lateral_accel_limit", config_.lateral_accel_limit_); + get_parameter("stopping_accel_limit_multiplier", config_.stopping_accel_limit_multiplier_); + get_parameter("min_maneuver_length", config_.min_maneuver_length_); + + RCLCPP_INFO_STREAM(rclcpp::get_logger("route_following_plugin"), "RouteFollowingPlugin Config: " << config_); // Setup publishers upcoming_lane_change_status_pub_ = create_publisher("upcoming_lane_change_status", 1); @@ -111,11 +125,11 @@ void setManeuverLaneletIds(carma_planning_msgs::msg::Maneuver& mvr, lanelet::Id std::bind(&RouteFollowingPlugin::twist_cb,this,std_ph::_1)); current_maneuver_plan_sub_ = create_subscription("maneuver_plan", 50, std::bind(&RouteFollowingPlugin::current_maneuver_plan_cb,this,std_ph::_1)); - + // set world model point form wm listener - wml_ = get_world_model_listener(); + wml_ = get_world_model_listener(); - wm_ = get_world_model(); + wm_ = get_world_model(); //set a route callback to update route and calculate maneuver wml_->setRouteCallback([this]() { diff --git a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp index 9f44627c4a..1dc161f164 100644 --- a/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp +++ b/stop_and_wait_plugin/src/stop_and_wait_plugin.cpp @@ -203,7 +203,7 @@ std::vector StopandWait::trajecto tpp.x = points[i].x(); tpp.y = points[i].y(); tpp.yaw = yaws[i]; - + tpp.planner_plugin_name = plugin_name_; tpp.controller_plugin_name = "default"; traj.push_back(tpp); @@ -376,6 +376,7 @@ std::vector StopandWait::compose_ { carma_planning_msgs::msg::TrajectoryPlanPoint new_point = traj.back(); new_point.target_time = rclcpp::Time(new_point.target_time) + rclcpp::Duration(config_.stop_timestep * 1e9); + new_point.planner_plugin_name = plugin_name_; traj.push_back(new_point); } diff --git a/subsystem_controllers/config/guidance_controller_config.yaml b/subsystem_controllers/config/guidance_controller_config.yaml index 0e7e75798c..b132b7b407 100644 --- a/subsystem_controllers/config/guidance_controller_config.yaml +++ b/subsystem_controllers/config/guidance_controller_config.yaml @@ -19,10 +19,9 @@ - /guidance/trajectory_executor - /guidance/twist_filter - /guidance/twist_gate - - /guidance/plugins/route_following_plugin # The minimal set of guidance plugins for system operation are route_following/inlanecruising/pure_pursuit + - /guidance/plugins/route_following_plugin # The minimal set of guidance plugins for system operation are route_following/inlanecruising/pure_pursuit_wrapper - /guidance/plugins/inlanecruising_plugin - /guidance/pure_pursuit_wrapper - - /guidance/pure_pursuit - /guidance/yield_plugin # List of nodes which will not be directly managed by this subsystem controller @@ -65,4 +64,5 @@ - /guidance/plugins/sci_strategic_plugin - /guidance/plugins/cooperative_lanechange - /guidance/plugins/platooning_tactical_plugin_node - - /guidance/plugins/yield_plugin \ No newline at end of file + - /guidance/plugins/yield_plugin + - /guidance/plugins/pure_pursuit_wrapper \ No newline at end of file diff --git a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp index 702bb5eeeb..d966d6c8c6 100644 --- a/subsystem_controllers/src/guidance_controller/plugin_manager.cpp +++ b/subsystem_controllers/src/guidance_controller/plugin_manager.cpp @@ -48,6 +48,8 @@ namespace subsystem_controllers for (const auto& p : required_plugins_) { bool is_ros1 = ros2_initial_plugins_.find(p) == ros2_initial_plugins_.end(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("subsystem_controllers"), "Added: " << p << ", as is_ros1:" << is_ros1); + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true, is_ros1); em_.update_entry(e); if (!is_ros1) @@ -56,6 +58,8 @@ namespace subsystem_controllers for (const auto& p : auto_activated_plugins_) { bool is_ros1 = ros2_initial_plugins_.find(p) == ros2_initial_plugins_.end(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("subsystem_controllers"), "Added: " << p << ", as is_ros1:" << is_ros1); + Entry e(false, false, p, carma_planning_msgs::msg::Plugin::UNKNOWN, "", true, is_ros1); em_.update_entry(e); if (!is_ros1) From cd9d3407dac3f90f7923eef0b78f40548e1f156d Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Thu, 22 Sep 2022 19:34:00 +0000 Subject: [PATCH 105/165] fix traj visualizer --- carma/launch/guidance.launch.py | 3 --- trajectory_visualizer/src/trajectory_visualizer.cpp | 2 ++ 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/carma/launch/guidance.launch.py b/carma/launch/guidance.launch.py index e7c03b600e..202cbd898b 100644 --- a/carma/launch/guidance.launch.py +++ b/carma/launch/guidance.launch.py @@ -79,9 +79,6 @@ def generate_launch_description(): port_drayage_plugin_param_file = os.path.join( get_package_share_directory('port_drayage_plugin'), 'config/parameters.yaml') - - trajectory_visualizer_param_file = os.path.join( - get_package_share_directory('trajectory_visualizer'), 'config/parameters.yaml') env_log_levels = EnvironmentVariable('CARMA_ROS_LOGGING_CONFIG', default_value='{ "default_level" : "WARN" }') diff --git a/trajectory_visualizer/src/trajectory_visualizer.cpp b/trajectory_visualizer/src/trajectory_visualizer.cpp index 887c131787..03e53c45ea 100644 --- a/trajectory_visualizer/src/trajectory_visualizer.cpp +++ b/trajectory_visualizer/src/trajectory_visualizer.cpp @@ -82,6 +82,8 @@ namespace trajectory_visualizer { if (i >= msg->trajectory_points.size()) { // If we need to delete previous points marker.action = visualization_msgs::msg::Marker::DELETE; + tmp_marker_array.markers.push_back(marker); + continue; } double max_speed = config_.max_speed * MPH_TO_MS; From 3e76222f0fb28fa5fa1f0418c9a7e5c29338080f Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 23 Sep 2022 07:11:16 -0700 Subject: [PATCH 106/165] ami_setup --- engineering_tools/aws_setup.bash | 140 +++++++++++++++++++++++++++++++ 1 file changed, 140 insertions(+) create mode 100644 engineering_tools/aws_setup.bash diff --git a/engineering_tools/aws_setup.bash b/engineering_tools/aws_setup.bash new file mode 100644 index 0000000000..365f38ee7b --- /dev/null +++ b/engineering_tools/aws_setup.bash @@ -0,0 +1,140 @@ +#!/bin/bash +sudo apt-get update && sudo apt-get upgrade -y linux-aws && sudo apt upgrade -y +sudo apt-get update && DEBIAN_FRONTEND=noninteractive sudo apt-get -y -o Dpkg::Options::=''--force-confdef'' -o Dpkg::Options::=''--force-confold'' dist-upgrade -y +sudo apt-get install -y python3 python3-dev python3-pip +pip3 install --upgrade awscli +pip3 install --upgrade pip boto3 requests +sudo apt-get install -y apt-transport-https ca-certificates curl software-properties-common +sudo apt-get install -y build-essential +sudo bash -c "sudo fallocate -l 512MB /var/swapfile && sudo chmod 600 /var/swapfile && sudo mkswap /var/swapfile && sudo echo '/var/swapfile swap swap defaults 0 0' >> /etc/fstab" +sudo sed -i 's|//Unattended-Upgrade::InstallOnShutdown "true";|Unattended-Upgrade::InstallOnShutdown "true";|' /etc/apt/apt.conf.d/50unattended-upgrades +DEBIAN_FRONTEND=noninteractive sudo apt-get install -y --no-install-recommends ubuntu-desktop gdm3 +DEBIAN_FRONTEND=noninteractive sudo apt-get install -y --no-install-recommends firefox xterm +sudo sed -i 's/^# AutomaticLogin/AutomaticLogin/' /etc/gdm3/custom.conf +sudo sed -i 's/user1/ubuntu/' /etc/gdm3/custom.conf +sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.screensaver idle-activation-enabled 'false'" +sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.screensaver lock-enabled 'false'" +sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.lockdown disable-lock-screen 'true'" +sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.session idle-delay 0" +sudo apt-get install -y python python-dev +chown -R ubuntu:ubuntu /home/ubuntu/.local +sudo rm /var/lib/update-notifier/updates-available +echo Installing DCV for Ubuntu 20.04 +wget https://d1uj6qtbmh3dt5.cloudfront.net/2022.1/Servers/nice-dcv-2022.1-13300-ubuntu2004-x86_64.tgz +tar xvfz nice-dcv-2022.1-13300-ubuntu2004-x86_64.tgz +cd nice-dcv-2022.1-13300-ubuntu2004-x86_64 +DEBIAN_FRONTEND=noninteractive sudo apt-get install -y +sudo apt --fix-broken install +sudo apt install -y ./nice-dcv-server_2022.1.13300-1_amd64.ubuntu2004.deb +sudo apt install -y ./nice-xdcv_2022.1.433-1_amd64.ubuntu2004.deb +sudo apt install -y ./nice-dcv-web-viewer_2022.1.13300-1_amd64.ubuntu2004.deb + +sudo usermod -aG video dcv + +cat << 'EOF' > ./dcv.conf +[license] +[log] +[display] +[connectivity] +web-port=8080 +web-use-https=false +[security] +authentication="none" +EOF + +sudo mv ./dcv.conf /etc/dcv/dcv.conf +sudo /usr/bin/dcvusbdriverinstaller --quiet +sudo su -l ubuntu -c dbus-launch gsettings set org.gnome.shell enabled-extensions "['ubuntu-dock@ubuntu.com']" +#sudo /sbin/iptables -A INPUT -p tcp ! -s localhost --dport 8080 -j DROP +sudo systemctl start dcvserver +sudo systemctl enable dcvserver + +sudo bash -c 'cat << "EOF" > /etc/systemd/system/dcvsession.service + [Unit] + Description=NICE DCV Session + After=dcvserver.service + + [Service] + User=ubuntu + ExecStart=/usr/bin/dcv create-session cloud9-session --owner ubuntu + + [Install] + WantedBy=multi-user.target +EOF' + +sudo systemctl daemon-reload +sudo systemctl enable dcvsession +sudo systemctl start dcvsession + +echo Installing ROS Noetic +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - +sudo apt update +sudo apt install -y ros-noetic-desktop +echo "[[ -e /opt/ros/noetic/setup.bash ]] && source /opt/ros/noetic/setup.bash" >> /home/ubuntu/.bashrc + +echo Installing ROS2 Foxy +sudo apt update && sudo apt install -y curl gnupg2 lsb-release +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null +sudo apt update +sudo apt install -y ros-foxy-desktop + +sudo apt-get install -y docker-compose +sleep 5 +sudo systemctl start docker + +cd ~ + +sudo apt-get update +sudo apt-get install -y lsb-release +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' +sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +sudo apt-get update +sudo apt-get install python3-vcstool +echo "source /usr/share/vcstool-completion/vcs.bash" >> ~/.bashrc + +sudo rm /usr/bin/docker-compose +DESTINATION=/usr/bin/docker-compose +sudo curl -L "https://github.com/docker/compose/releases/download/1.29.1/docker-compose-$(uname -s)-$(uname -m)" -o $DESTINATION +sudo chmod 755 $DESTINATION + +sudo apt install -y chromium-browser + +( +echo "parse_git_branch() {" +echo " git branch 2> /dev/null | sed -e '/^[^*]/d' -e 's/* \(.*\)/ (\1)/'" +echo "}" +echo "#Add git branch and timestamp to bash commands" +echo export PS1='"\t \u@\h \[\033[32m\]\w\[\033[33m\]\$(parse_git_branch)\[\033[00m\] $ "' +) >> ~/.bashrc + +git clone https://github.com/usdot-fhwa-stol/carma-config.git + +sudo curl -L https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/engineering_tools/opt_carma_setup.bash > ~/opt_carma_setup.bash +sudo bash ~/opt_carma_setup.bash ~/carma-config/example_calibration_folder/vehicle/ +rm ~/opt_carma_setup.bash + +sudo curl -o /usr/bin/carma -L https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/engineering_tools/carma +sudo chmod ugo+x /usr/bin/carma +sudo curl -o /etc/bash_completion.d/__carma_autocomplete -L https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/engineering_tools/__carma_autocomplete +sudo chmod ugo+x /etc/bash_completion.d/__carma_autocomplete + +docker pull usdotfhwastol/carma-xil-cosimulation:1.0.0-beta +docker pull usdotfhwastol/carma-carla-integration:carma-carla-1.0 + +carma config install usdotfhwastoldev/carma-config:aws-test-carla-integration +carma config set usdotfhwastoldev/carma-config:aws-test-carla-integration + +cd /home/ubuntu +mkdir carma_ws +cd carma_ws +mkdir src +cd src + +# TODO make branches a variable +git clone https://github.com/usdot-fhwa-stol/carma-carla-integration.git --branch main +git clone https://github.com/usdot-fhwa-stol/carma-simulation.git --branch update_co_sim + + +cd /home/ubuntu From 647dab9f642776a7347aa5b13b8b4979fe5d6c70 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 23 Sep 2022 07:38:27 -0700 Subject: [PATCH 107/165] increase sleep --- engineering_tools/aws_setup.bash | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/engineering_tools/aws_setup.bash b/engineering_tools/aws_setup.bash index 365f38ee7b..06264a9009 100644 --- a/engineering_tools/aws_setup.bash +++ b/engineering_tools/aws_setup.bash @@ -81,7 +81,7 @@ sudo apt update sudo apt install -y ros-foxy-desktop sudo apt-get install -y docker-compose -sleep 5 +sleep 10 sudo systemctl start docker cd ~ From 122883d857190554c4e82b280b5f08b58d6a3cc0 Mon Sep 17 00:00:00 2001 From: Michael McConnell Date: Fri, 23 Sep 2022 17:35:42 +0000 Subject: [PATCH 108/165] switch to ubuntu 18.04 --- engineering_tools/aws_setup.bash | 49 +++++++++++++++++--------------- 1 file changed, 26 insertions(+), 23 deletions(-) diff --git a/engineering_tools/aws_setup.bash b/engineering_tools/aws_setup.bash index 06264a9009..20720f06d9 100644 --- a/engineering_tools/aws_setup.bash +++ b/engineering_tools/aws_setup.bash @@ -1,4 +1,13 @@ #!/bin/bash + +# exit when any command fails +set -e + +# keep track of the last executed command +trap 'last_command=$current_command; current_command=$BASH_COMMAND' DEBUG +# echo an error message before exiting +trap 'echo "\"${last_command}\" command filed with exit code $?."' EXIT + sudo apt-get update && sudo apt-get upgrade -y linux-aws && sudo apt upgrade -y sudo apt-get update && DEBIAN_FRONTEND=noninteractive sudo apt-get -y -o Dpkg::Options::=''--force-confdef'' -o Dpkg::Options::=''--force-confold'' dist-upgrade -y sudo apt-get install -y python3 python3-dev python3-pip @@ -8,10 +17,12 @@ sudo apt-get install -y apt-transport-https ca-certificates curl software-proper sudo apt-get install -y build-essential sudo bash -c "sudo fallocate -l 512MB /var/swapfile && sudo chmod 600 /var/swapfile && sudo mkswap /var/swapfile && sudo echo '/var/swapfile swap swap defaults 0 0' >> /etc/fstab" sudo sed -i 's|//Unattended-Upgrade::InstallOnShutdown "true";|Unattended-Upgrade::InstallOnShutdown "true";|' /etc/apt/apt.conf.d/50unattended-upgrades -DEBIAN_FRONTEND=noninteractive sudo apt-get install -y --no-install-recommends ubuntu-desktop gdm3 +DEBIAN_FRONTEND=noninteractive sudo apt-get install -y --no-install-recommends ubuntu-desktop lightdm DEBIAN_FRONTEND=noninteractive sudo apt-get install -y --no-install-recommends firefox xterm sudo sed -i 's/^# AutomaticLogin/AutomaticLogin/' /etc/gdm3/custom.conf sudo sed -i 's/user1/ubuntu/' /etc/gdm3/custom.conf + + sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.screensaver idle-activation-enabled 'false'" sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.screensaver lock-enabled 'false'" sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.lockdown disable-lock-screen 'true'" @@ -19,15 +30,15 @@ sudo su -l ubuntu -c "dbus-launch gsettings set org.gnome.desktop.session idle-d sudo apt-get install -y python python-dev chown -R ubuntu:ubuntu /home/ubuntu/.local sudo rm /var/lib/update-notifier/updates-available -echo Installing DCV for Ubuntu 20.04 -wget https://d1uj6qtbmh3dt5.cloudfront.net/2022.1/Servers/nice-dcv-2022.1-13300-ubuntu2004-x86_64.tgz -tar xvfz nice-dcv-2022.1-13300-ubuntu2004-x86_64.tgz -cd nice-dcv-2022.1-13300-ubuntu2004-x86_64 -DEBIAN_FRONTEND=noninteractive sudo apt-get install -y -sudo apt --fix-broken install -sudo apt install -y ./nice-dcv-server_2022.1.13300-1_amd64.ubuntu2004.deb -sudo apt install -y ./nice-xdcv_2022.1.433-1_amd64.ubuntu2004.deb -sudo apt install -y ./nice-dcv-web-viewer_2022.1.13300-1_amd64.ubuntu2004.deb +echo Installing DCV for Ubuntu 18.04 + +wget https://d1uj6qtbmh3dt5.cloudfront.net/2020.1/Servers/nice-dcv-2020.1-9012-ubuntu1804-x86_64.tgz && echo "7569c95465743b512f1ab191e58ea09777353b401c1ec130ee8ea344e00f8900 nice-dcv-2020.1-9012-ubuntu1804-x86_64.tgz" | sha256sum -c && tar -xvzf nice-dcv-2020.1-9012-ubuntu1804-x86_64.tgz && rm nice-dcv-2020.1-9012-ubuntu1804-x86_64.tgz +cd nice-dcv-2020.1-9012-ubuntu1804-x86_64 && \ +DEBIAN_FRONTEND=noninteractive apt-get install -y \ +./nice-dcv-server_2020.1.9012-1_amd64.ubuntu1804.deb \ +./nice-xdcv_2020.1.338-1_amd64.ubuntu1804.deb + + sudo usermod -aG video dcv @@ -66,19 +77,11 @@ sudo systemctl daemon-reload sudo systemctl enable dcvsession sudo systemctl start dcvsession -echo Installing ROS Noetic -sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' -curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - -sudo apt update -sudo apt install -y ros-noetic-desktop -echo "[[ -e /opt/ros/noetic/setup.bash ]] && source /opt/ros/noetic/setup.bash" >> /home/ubuntu/.bashrc - -echo Installing ROS2 Foxy -sudo apt update && sudo apt install -y curl gnupg2 lsb-release -sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg -echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(source /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null -sudo apt update -sudo apt install -y ros-foxy-desktop +echo Installing ROS Melodic +git clone https://github.com/aws-robotics/aws-robomaker-sample-application-helloworld.git -b ros1 && cd aws-robomaker-sample-application-helloworld/ && bash -c scripts/setup.sh --install-ros melodic +cd /home/ubuntu +rm -rf aws-robomaker-sample-application-helloworld/ +echo "[[ -e /opt/ros/melodic/setup.sh ]] && source /opt/ros/melodic/setup.sh" >> /home/ubuntu/.bashrc sudo apt-get install -y docker-compose sleep 10 From 95edd8897b806505ac5d6276635d5d4f39ae4cbe Mon Sep 17 00:00:00 2001 From: Alex Frye Date: Thu, 29 Sep 2022 06:28:23 -0700 Subject: [PATCH 109/165] Fix/develop localization (#1944) Description Fixed one bug, where the timeout from state DEGRADED_NO_LIDAR_FIX would crash after completing, because in its setup it was partially written on top of the timeout from state INITIALIZING. The actual fix is just setting the timer id as a new id. Also fixed logging issues with the localization manager. previously, logging was different in each file and didn't work from the transition table file. After switching each call for the logger to rclcpp::get_logger("localization.localization_manager"), "string to print");, the logging works just fine. Related Issue https://usdot-carma.atlassian.net/browse/CAR-5631 and resolves #1911 Motivation and Context localization manager was crashing 20 seconds after setting a pose in rviz, causing the localization to no longer update properly. How Has This Been Tested? Started carma, localized with rviz, then selected a route to drive from underneath the tree to the east intersection, and the localization manager stayed up and it worked great. Co-authored-by: Misheel Bayartsengel --- .../LocalizationManager.hpp | 299 ++++++------ .../LocalizationManagerConfig.hpp | 92 ++-- .../LocalizationTransitionTable.hpp | 118 ++--- .../LocalizationTypes.hpp | 107 +++-- .../localization_manager_node.hpp | 121 +++-- .../src/LocalizationManager.cpp | 443 +++++++++-------- .../src/LocalizationTransitionTable.cpp | 446 +++++++++--------- .../src/LocalizationTypes.cpp | 104 ++-- .../src/localization_manager_node.cpp | 73 ++- localization_manager/src/main.cpp | 18 +- 10 files changed, 903 insertions(+), 918 deletions(-) diff --git a/localization_manager/include/localization_manager/LocalizationManager.hpp b/localization_manager/include/localization_manager/LocalizationManager.hpp index ecf057d19c..4b89ffe7d9 100644 --- a/localization_manager/include/localization_manager/LocalizationManager.hpp +++ b/localization_manager/include/localization_manager/LocalizationManager.hpp @@ -34,157 +34,154 @@ namespace localization_manager { -/** - * \brief Primary logic class for the localization manager node. - */ -class LocalizationManager -{ - -public: - using PosePublisher = std::function; - using ManagedInitialPosePublisher = std::function; - using StatePublisher = std::function; - using TimerUniquePtr = std::unique_ptr; - - /** - * \brief Constructor - * - * \param pose_pub A callback to trigger publication of the selected pose - * \param state_pub A callback to trigger publication of the localization state - * \param initialpose_pub A callback to trigger publication of the intial pose - * \param config The configuration settings to use for this manager - * \param logger Logger interface of node calling manager object - * \param node_timers Timer interface of node calling manager object - */ - LocalizationManager(PosePublisher pose_pub, StatePublisher state_pub, - ManagedInitialPosePublisher initialpose_pub, - const LocalizationManagerConfig& config, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, - std::unique_ptr timer_factory); - - /** - * \brief Callback for new GNSS messages - * - * \param msg The pose of vehicle in map frame provided by GNSS - */ - void gnssPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); - - /** - * \brief Synced callback for the NDT reported pose and status messages - * - * \param pose The pose reported by NDT matching of the vehicle in the map frame - * \param stats The stats reported by NDT matching for the accuracy of the provided pose - */ - void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, - const autoware_msgs::msg::NDTStat::ConstPtr stats); - - /** - * \brief Callback for SystemAlert data. Used to check for lidar failure - * - * \param alert The alert message to evaluate - */ - void systemAlertCallback(const carma_msgs::msg::SystemAlert::SharedPtr alert); - - /** - * \brief Callback for the initial pose provided by Rviz or some external localization initializer - * - * \param msg The initial pose message - */ - void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - - /** - * \brief Timer callback that controls the publication of the selected pose and status report - * - */ - void posePubTick(); - - /** - * \brief Callback for when a new state was triggered. Allows creation of entry/exit behavior for states - * - * \param prev_state The previous state - * \param new_state The new current state. This should never equal prev_state. - * \param signal The signal that triggered the state transition - */ - void stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal); - - /** - * \brief Callback for timeouts. Used to trigger timeout signals for the state machine. - * - * \param origin_state The state that was the current state when the timer that triggered this callback was setup - */ - void timerCallback(const LocalizationState origin_state); - /** - * \brief Returns the current localization state - * - * \return The current localization state + * \brief Primary logic class for the localization manager node. */ - LocalizationState getState() const; - - /** - * @brief Clears the expired timers from the memory of this scheduler - */ - void clearTimers(); - -private: - //! The set of strings which mark a lidar failure in a system alert message - static const std::unordered_set LIDAR_FAILURE_STRINGS; // Static const container defined in cpp file - - PosePublisher pose_pub_; - StatePublisher state_pub_; - ManagedInitialPosePublisher initialpose_pub_; - - LocalizationManagerConfig config_; - - LocalizationTransitionTable transition_table_; - - boost::optional prev_ndt_stamp_; - - int lidar_init_sequential_timesteps_counter_ = 0; - bool is_sequential_ = false; - boost::optional last_raw_gnss_value_; - boost::optional gnss_offset_; - boost::optional current_pose_; - - // Logger interface - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; - - // Using timer factory - std::mutex mutex_; - std::unique_ptr timer_factory_; - std::unordered_map> timers_; - TimerUniquePtr current_timer_; - uint32_t next_id_ = 0; // Timer id counter - uint32_t current_timer_id_; - rcl_clock_type_t timer_clock_type_ = RCL_SYSTEM_TIME; - - - - /** - * \brief Helper function to both compute the NDT Frequency and update the previous pose timestamp - * - * \param new_stamp The new pose timestamp - * - * \return The computed instantaneous frequency in Hz - */ - double computeNDTFreq(const rclcpp::Time& new_stamp); - - /** - * \brief Helper function to compute the instantaneous frequency between two times - * - * \param old_stamp The old timestamp - * \param new_stamp The new timestamp - * - * \return The computed instantaneous frequency in Hz - */ - double computeFreq(const rclcpp::Time& old_stamp, const rclcpp::Time& new_stamp) const; - - /** - * @brief Generates the next id to be used for a timer - * - * @return The next available timer id - */ - uint32_t nextId(); - -}; + class LocalizationManager + { + + public: + using PosePublisher = std::function; + using ManagedInitialPosePublisher = std::function; + using StatePublisher = std::function; + using TimerUniquePtr = std::unique_ptr; + + /** + * \brief Constructor + * + * \param pose_pub A callback to trigger publication of the selected pose + * \param state_pub A callback to trigger publication of the localization state + * \param initialpose_pub A callback to trigger publication of the intial pose + * \param config The configuration settings to use for this manager + * \param logger Logger interface of node calling manager object + * \param node_timers Timer interface of node calling manager object + */ + LocalizationManager(PosePublisher pose_pub, StatePublisher state_pub, + ManagedInitialPosePublisher initialpose_pub, + const LocalizationManagerConfig &config, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, + std::unique_ptr timer_factory); + + /** + * \brief Callback for new GNSS messages + * + * \param msg The pose of vehicle in map frame provided by GNSS + */ + void gnssPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + + /** + * \brief Synced callback for the NDT reported pose and status messages + * + * \param pose The pose reported by NDT matching of the vehicle in the map frame + * \param stats The stats reported by NDT matching for the accuracy of the provided pose + */ + void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, + const autoware_msgs::msg::NDTStat::ConstPtr stats); + + /** + * \brief Callback for SystemAlert data. Used to check for lidar failure + * + * \param alert The alert message to evaluate + */ + void systemAlertCallback(const carma_msgs::msg::SystemAlert::SharedPtr alert); + + /** + * \brief Callback for the initial pose provided by Rviz or some external localization initializer + * + * \param msg The initial pose message + */ + void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + + /** + * \brief Timer callback that controls the publication of the selected pose and status report + * + */ + void posePubTick(); + + /** + * \brief Callback for when a new state was triggered. Allows creation of entry/exit behavior for states + * + * \param prev_state The previous state + * \param new_state The new current state. This should never equal prev_state. + * \param signal The signal that triggered the state transition + */ + void stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal); + + /** + * \brief Callback for timeouts. Used to trigger timeout signals for the state machine. + * + * \param origin_state The state that was the current state when the timer that triggered this callback was setup + */ + void timerCallback(const LocalizationState origin_state); + + /** + * \brief Returns the current localization state + * + * \return The current localization state + */ + LocalizationState getState() const; + + /** + * @brief Clears the expired timers from the memory of this scheduler + */ + void clearTimers(); + + private: + //! The set of strings which mark a lidar failure in a system alert message + static const std::unordered_set LIDAR_FAILURE_STRINGS; // Static const container defined in cpp file + + PosePublisher pose_pub_; + StatePublisher state_pub_; + ManagedInitialPosePublisher initialpose_pub_; + + LocalizationManagerConfig config_; + + LocalizationTransitionTable transition_table_; + + boost::optional prev_ndt_stamp_; + + int lidar_init_sequential_timesteps_counter_ = 0; + bool is_sequential_ = false; + boost::optional last_raw_gnss_value_; + boost::optional gnss_offset_; + boost::optional current_pose_; + + // Logger interface + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_; + + // Using timer factory + std::mutex mutex_; + std::unique_ptr timer_factory_; + std::unordered_map> timers_; + TimerUniquePtr current_timer_; + uint32_t next_id_ = 0; // Timer id counter + uint32_t current_timer_id_; + rcl_clock_type_t timer_clock_type_ = RCL_SYSTEM_TIME; + + /** + * \brief Helper function to both compute the NDT Frequency and update the previous pose timestamp + * + * \param new_stamp The new pose timestamp + * + * \return The computed instantaneous frequency in Hz + */ + double computeNDTFreq(const rclcpp::Time &new_stamp); + + /** + * \brief Helper function to compute the instantaneous frequency between two times + * + * \param old_stamp The old timestamp + * \param new_stamp The new timestamp + * + * \return The computed instantaneous frequency in Hz + */ + double computeFreq(const rclcpp::Time &old_stamp, const rclcpp::Time &new_stamp) const; + + /** + * @brief Generates the next id to be used for a timer + * + * @return The next available timer id + */ + uint32_t nextId(); + }; } \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp b/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp index 7617437fa2..16e62adc14 100644 --- a/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp +++ b/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp @@ -20,50 +20,50 @@ namespace localization_manager { -//! @brief Struct to store the configuration settings for the LocalizationManager class -struct LocalizationManagerConfig -{ - //! NDT Fitness score above which the localization is considered in a degraded state - double fitness_score_degraded_threshold = 20.0; - //! NDT Fitness score above which the localization is considered in a fault state and NDT matching can no longer be - //! used. - double fitness_score_fault_threshold = 10000.0; - //! NDT solution frequency below which the localization is considered in a degraded state - double ndt_frequency_degraded_threshold = 8.0; - //! NDT solution frequency below which the localization is considered in a fault state and NDT matching can no longer - //! be used. - double ndt_frequency_fault_threshold = 0.66; - //! Timeout in ms for auto initialization. - //! If initialization cannot be completed in this time user action will be requested. - int auto_initialization_timeout = 30000; - //! Timeout in ms for GNSS only operation. Ignored when in GNSS mode. - int gnss_only_operation_timeout = 20000; - //! Integer: Maximum allowed number of sequential timesteps to let lidar initialize before switching to GPS only mode - //! NOTE: Only used in GNSS only with NDT initialization mode - int sequential_timesteps_until_gps_operation = 5; - //! GNSS Data timeout. If exceeded the system will assume the GNSS is no longer functional. Units are ms - int gnss_data_timeout = 500; - //! Localization mode to use - int localization_mode = static_cast(LocalizerMode::AUTO_WITHOUT_TIMEOUT); - //! Selected pose publication rate - double pose_pub_rate = 10.0; - - // Stream operator for this config - friend std::ostream &operator<<(std::ostream &output, const LocalizationManagerConfig &c) + //! @brief Struct to store the configuration settings for the LocalizationManager class + struct LocalizationManagerConfig { - output << "localization_manager::Config { " << std::endl - << "fitness_score_degraded_threshold: " << c.fitness_score_degraded_threshold << std::endl - << "fitness_score_fault_threshold: " << c.fitness_score_fault_threshold << std::endl - << "ndt_frequency_degraded_threshold: " << c.ndt_frequency_degraded_threshold << std::endl - << "ndt_frequency_fault_threshold: " << c.ndt_frequency_fault_threshold << std::endl - << "auto_initialization_timeout: " << c.auto_initialization_timeout << std::endl - << "gnss_only_operation_timeout: " << c.gnss_only_operation_timeout << std::endl - << "sequential_timesteps_until_gps_operation: " << c.sequential_timesteps_until_gps_operation << std::endl - << "gnss_data_timeout: " << c.gnss_data_timeout << std::endl - << "localization_mode: " << static_cast(c.localization_mode) << std::endl - << "pose_pub_rate: " << c.pose_pub_rate << std::endl - << "}" << std::endl; - return output; - } -}; -} //namespace localization_manager \ No newline at end of file + //! NDT Fitness score above which the localization is considered in a degraded state + double fitness_score_degraded_threshold = 20.0; + //! NDT Fitness score above which the localization is considered in a fault state and NDT matching can no longer be + //! used. + double fitness_score_fault_threshold = 10000.0; + //! NDT solution frequency below which the localization is considered in a degraded state + double ndt_frequency_degraded_threshold = 8.0; + //! NDT solution frequency below which the localization is considered in a fault state and NDT matching can no longer + //! be used. + double ndt_frequency_fault_threshold = 0.66; + //! Timeout in ms for auto initialization. + //! If initialization cannot be completed in this time user action will be requested. + int auto_initialization_timeout = 30000; + //! Timeout in ms for GNSS only operation. Ignored when in GNSS mode. + int gnss_only_operation_timeout = 20000; + //! Integer: Maximum allowed number of sequential timesteps to let lidar initialize before switching to GPS only mode + //! NOTE: Only used in GNSS only with NDT initialization mode + int sequential_timesteps_until_gps_operation = 5; + //! GNSS Data timeout. If exceeded the system will assume the GNSS is no longer functional. Units are ms + int gnss_data_timeout = 500; + //! Localization mode to use + int localization_mode = static_cast(LocalizerMode::AUTO_WITHOUT_TIMEOUT); + //! Selected pose publication rate + double pose_pub_rate = 10.0; + + // Stream operator for this config + friend std::ostream &operator<<(std::ostream &output, const LocalizationManagerConfig &c) + { + output << "localization_manager::Config { " << std::endl + << "fitness_score_degraded_threshold: " << c.fitness_score_degraded_threshold << std::endl + << "fitness_score_fault_threshold: " << c.fitness_score_fault_threshold << std::endl + << "ndt_frequency_degraded_threshold: " << c.ndt_frequency_degraded_threshold << std::endl + << "ndt_frequency_fault_threshold: " << c.ndt_frequency_fault_threshold << std::endl + << "auto_initialization_timeout: " << c.auto_initialization_timeout << std::endl + << "gnss_only_operation_timeout: " << c.gnss_only_operation_timeout << std::endl + << "sequential_timesteps_until_gps_operation: " << c.sequential_timesteps_until_gps_operation << std::endl + << "gnss_data_timeout: " << c.gnss_data_timeout << std::endl + << "localization_mode: " << static_cast(c.localization_mode) << std::endl + << "pose_pub_rate: " << c.pose_pub_rate << std::endl + << "}" << std::endl; + return output; + } + }; +} // namespace localization_manager \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationTransitionTable.hpp b/localization_manager/include/localization_manager/LocalizationTransitionTable.hpp index 077058874b..a92212b9d0 100644 --- a/localization_manager/include/localization_manager/LocalizationTransitionTable.hpp +++ b/localization_manager/include/localization_manager/LocalizationTransitionTable.hpp @@ -20,72 +20,72 @@ namespace localization_manager { -/** - * \brief Class defining the state transition table behavior for the LocalizationManager - */ -class LocalizationTransitionTable -{ -public: - using TransitionCallback = - std::function; - - /** - * \brief Constructor - * - * \param mode Defines the operational mode of the state machine which modifies some of the state transitions - */ - LocalizationTransitionTable(LocalizerMode mode); - /** - * \brief Returns the current state - * \return Current state + * \brief Class defining the state transition table behavior for the LocalizationManager */ - LocalizationState getState() const; + class LocalizationTransitionTable + { + public: + using TransitionCallback = + std::function; - /** - * \brief Trigger signal for the transition table. - * - * \param signal The signal for the transition table to evaluate - */ - void signal(LocalizationSignal signal); + /** + * \brief Constructor + * + * \param mode Defines the operational mode of the state machine which modifies some of the state transitions + */ + LocalizationTransitionTable(LocalizerMode mode); - /** - * \brief Callback setting function. The provided callback will be triggered any time the current state changes to a - * new state. - * - * \param cb The callback function which will be provided with the previous state, new current state, and the signal - * which caused the transition. - */ - void setTransitionCallback(TransitionCallback cb); + /** + * \brief Returns the current state + * \return Current state + */ + LocalizationState getState() const; -private: - //! Current state. This state should only ever be set using the setAndLogState() function. - LocalizationState state_ = LocalizationState::UNINITIALIZED; + /** + * \brief Trigger signal for the transition table. + * + * \param signal The signal for the transition table to evaluate + */ + void signal(LocalizationSignal signal); - LocalizerMode mode_ = LocalizerMode::AUTO_WITHOUT_TIMEOUT; + /** + * \brief Callback setting function. The provided callback will be triggered any time the current state changes to a + * new state. + * + * \param cb The callback function which will be provided with the previous state, new current state, and the signal + * which caused the transition. + */ + void setTransitionCallback(TransitionCallback cb); - TransitionCallback transition_callback_; + private: + //! Current state. This state should only ever be set using the setAndLogState() function. + LocalizationState state_ = LocalizationState::UNINITIALIZED; - // Helper functions for processing each the provided signal based on the current state - void signalWhenUNINITIALIZED(LocalizationSignal signal); - void signalWhenINITIALIZING(LocalizationSignal signal); - void signalWhenOPERATIONAL(LocalizationSignal signal); - void signalWhenDEGRADED(LocalizationSignal signal); - void signalWhenDEGRADED_NO_LIDAR_FIX(LocalizationSignal signal); - void signalWhenAWAIT_MANUAL_INITIALIZATION(LocalizationSignal signal); + LocalizerMode mode_ = LocalizerMode::AUTO_WITHOUT_TIMEOUT; - /** - * \brief Helper function for logging the provide signal - * \param signal The signal to be logged - */ - void logDebugSignal(LocalizationSignal signal) const; + TransitionCallback transition_callback_; - /** - * \brief Function to change the current state and log the details of the transition. - * - * \param new_state The state to set. - * \param source_signal The signal which caused the new_state to be set - */ - void setAndLogState(LocalizationState new_state, LocalizationSignal source_signal); -}; -} //namespace localization_manager \ No newline at end of file + // Helper functions for processing each the provided signal based on the current state + void signalWhenUNINITIALIZED(LocalizationSignal signal); + void signalWhenINITIALIZING(LocalizationSignal signal); + void signalWhenOPERATIONAL(LocalizationSignal signal); + void signalWhenDEGRADED(LocalizationSignal signal); + void signalWhenDEGRADED_NO_LIDAR_FIX(LocalizationSignal signal); + void signalWhenAWAIT_MANUAL_INITIALIZATION(LocalizationSignal signal); + + /** + * \brief Helper function for logging the provide signal + * \param signal The signal to be logged + */ + void logDebugSignal(LocalizationSignal signal) const; + + /** + * \brief Function to change the current state and log the details of the transition. + * + * \param new_state The state to set. + * \param source_signal The signal which caused the new_state to be set + */ + void setAndLogState(LocalizationState new_state, LocalizationSignal source_signal); + }; +} // namespace localization_manager \ No newline at end of file diff --git a/localization_manager/include/localization_manager/LocalizationTypes.hpp b/localization_manager/include/localization_manager/LocalizationTypes.hpp index 332ee8c211..da40455db4 100644 --- a/localization_manager/include/localization_manager/LocalizationTypes.hpp +++ b/localization_manager/include/localization_manager/LocalizationTypes.hpp @@ -21,64 +21,63 @@ namespace localization_manager { -//! @brief Enum describing the possible operational modes of the LocalizationManager -enum LocalizerMode -{ - NDT = 0, // NDT only operation - GNSS = 1, // GNSS only operation - AUTO_WITH_TIMEOUT = 2, // NDT operation with support for GPS fallback that will timeout - AUTO_WITHOUT_TIMEOUT = 3, // NDT operation with support for GPS fallback that will not timeout - GNSS_WITH_NDT_INIT = 4, // GNSS only operation with NDT initialization, switching to GNSS after 5 sequential timesteps of OPERATIONAL NDT -}; + //! @brief Enum describing the possible operational modes of the LocalizationManager + enum LocalizerMode + { + NDT = 0, // NDT only operation + GNSS = 1, // GNSS only operation + AUTO_WITH_TIMEOUT = 2, // NDT operation with support for GPS fallback that will timeout + AUTO_WITHOUT_TIMEOUT = 3, // NDT operation with support for GPS fallback that will not timeout + GNSS_WITH_NDT_INIT = 4, // GNSS only operation with NDT initialization, switching to GNSS after 5 sequential timesteps of OPERATIONAL NDT + }; -/** - * \brief Stream operator for LocalizerMode enum. - */ -std::ostream& operator<<(std::ostream& os, LocalizerMode m); + /** + * \brief Stream operator for LocalizerMode enum. + */ + std::ostream &operator<<(std::ostream &os, LocalizerMode m); -//! @brief Enum describing the possible states of the localization system -enum class LocalizationState -{ - UNINITIALIZED, - INITIALIZING, - OPERATIONAL, - DEGRADED, - DEGRADED_NO_LIDAR_FIX, - AWAIT_MANUAL_INITIALIZATION, -}; + //! @brief Enum describing the possible states of the localization system + enum class LocalizationState + { + UNINITIALIZED, + INITIALIZING, + OPERATIONAL, + DEGRADED, + DEGRADED_NO_LIDAR_FIX, + AWAIT_MANUAL_INITIALIZATION, + }; -/** - * \brief Stream operator for LocalizationState enum. - */ -std::ostream& operator<<(std::ostream& os, LocalizationState s); + /** + * \brief Stream operator for LocalizationState enum. + */ + std::ostream &operator<<(std::ostream &os, LocalizationState s); -/** - * \brief Helper function to convert LocalizationState objects into LocalizationStatusReport ROS messages - * - * \param state The state to convert - * \param stamp The timestamp to set for the message header - * - * \return The initialized report message - */ -carma_localization_msgs::msg::LocalizationStatusReport stateToMsg(LocalizationState state, const rclcpp::Time& stamp); + /** + * \brief Helper function to convert LocalizationState objects into LocalizationStatusReport ROS messages + * + * \param state The state to convert + * \param stamp The timestamp to set for the message header + * + * \return The initialized report message + */ + carma_localization_msgs::msg::LocalizationStatusReport stateToMsg(LocalizationState state, const rclcpp::Time &stamp); + //! @brief Enum describing the possible signals to change the current LocalizationState + enum class LocalizationSignal + { + INITIAL_POSE, + GOOD_NDT_FREQ_AND_FITNESS_SCORE, + POOR_NDT_FREQ_OR_FITNESS_SCORE, + UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE, + TIMEOUT, + LIDAR_SENSOR_FAILURE, + LIDAR_INITIALIZED_SWITCH_TO_GPS, + GNSS_DATA_TIMEOUT + }; -//! @brief Enum describing the possible signals to change the current LocalizationState -enum class LocalizationSignal -{ - INITIAL_POSE, - GOOD_NDT_FREQ_AND_FITNESS_SCORE, - POOR_NDT_FREQ_OR_FITNESS_SCORE, - UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE, - TIMEOUT, - LIDAR_SENSOR_FAILURE, - LIDAR_INITIALIZED_SWITCH_TO_GPS, - GNSS_DATA_TIMEOUT -}; - -/** - * \brief Stream operator for LocalizationSignal enum. - */ -std::ostream& operator<<(std::ostream& os, LocalizationSignal s); + /** + * \brief Stream operator for LocalizationSignal enum. + */ + std::ostream &operator<<(std::ostream &os, LocalizationSignal s); -} //namespace localization_manager \ No newline at end of file +} // namespace localization_manager \ No newline at end of file diff --git a/localization_manager/include/localization_manager/localization_manager_node.hpp b/localization_manager/include/localization_manager/localization_manager_node.hpp index 41af9d48ff..5d5be433a9 100644 --- a/localization_manager/include/localization_manager/localization_manager_node.hpp +++ b/localization_manager/include/localization_manager/localization_manager_node.hpp @@ -39,74 +39,73 @@ namespace localization_manager { /** - * \brief Core execution node for this package - */ + * \brief Core execution node for this package + */ - class Node : public carma_ros2_utils::CarmaLifecycleNode - { + class Node : public carma_ros2_utils::CarmaLifecycleNode + { private: - // Subscribers - message_filters::Subscriber ndt_pose_sub_; - message_filters::Subscriber ndt_score_sub_; - carma_ros2_utils::SubPtr gnss_pose_sub_; - carma_ros2_utils::SubPtr initialpose_sub_; - carma_ros2_utils::SubPtr system_alert_sub_; + // Subscribers + message_filters::Subscriber ndt_pose_sub_; + message_filters::Subscriber ndt_score_sub_; + carma_ros2_utils::SubPtr gnss_pose_sub_; + carma_ros2_utils::SubPtr initialpose_sub_; + carma_ros2_utils::SubPtr system_alert_sub_; - // Publishers - carma_ros2_utils::PubPtr pose_pub_; - carma_ros2_utils::PubPtr state_pub_; - carma_ros2_utils::PubPtr managed_initial_pose_pub_; + // Publishers + carma_ros2_utils::PubPtr pose_pub_; + carma_ros2_utils::PubPtr state_pub_; + carma_ros2_utils::PubPtr managed_initial_pose_pub_; - // Timers - rclcpp::TimerBase::SharedPtr pose_timer_; + // Timers + rclcpp::TimerBase::SharedPtr pose_timer_; - // Node configuration - LocalizationManagerConfig config_; + // Node configuration + LocalizationManagerConfig config_; - // Worker object - std::unique_ptr manager_; + // Worker object + std::unique_ptr manager_; - // Message filters policies (TimeSynchronizer by default uses ExactTime Policy) - typedef message_filters::TimeSynchronizer TimeSynchronizer; - std::shared_ptr pose_stats_synchronizer_; + // Message filters policies (TimeSynchronizer by default uses ExactTime Policy) + typedef message_filters::TimeSynchronizer TimeSynchronizer; + std::shared_ptr pose_stats_synchronizer_; public: - /** - * \brief Node constructor - */ - explicit Node(const rclcpp::NodeOptions &); - - /** - * \brief Callback to publish the selected pose - * \param msg The pose to publish - */ - void publishPoseStamped(const geometry_msgs::msg::PoseStamped& msg) const; - - /** - * \brief Callback to publish the provided localization status report - * \param msg The report to publish - */ - void publishStatus(const carma_localization_msgs::msg::LocalizationStatusReport& msg) const; - - /** - * \brief Callback to publish the initial pose deemed suitable to intialize NDT - * \param msg The msg to publish - */ - void publishManagedInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) const; - - /** - * \brief Synchronized callback for pose and stats data for usage with message_filters. - * Provides exception handling. - * \param pose The received pose message - * \param stats The received stats message - */ - void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, - const autoware_msgs::msg::NDTStat::ConstPtr stats); - - //// - // Overrides - //// - carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); - - }; + /** + * \brief Node constructor + */ + explicit Node(const rclcpp::NodeOptions &); + + /** + * \brief Callback to publish the selected pose + * \param msg The pose to publish + */ + void publishPoseStamped(const geometry_msgs::msg::PoseStamped &msg) const; + + /** + * \brief Callback to publish the provided localization status report + * \param msg The report to publish + */ + void publishStatus(const carma_localization_msgs::msg::LocalizationStatusReport &msg) const; + + /** + * \brief Callback to publish the initial pose deemed suitable to intialize NDT + * \param msg The msg to publish + */ + void publishManagedInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped &msg) const; + + /** + * \brief Synchronized callback for pose and stats data for usage with message_filters. + * Provides exception handling. + * \param pose The received pose message + * \param stats The received stats message + */ + void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, + const autoware_msgs::msg::NDTStat::ConstPtr stats); + + //// + // Overrides + //// + carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &); + }; } \ No newline at end of file diff --git a/localization_manager/src/LocalizationManager.cpp b/localization_manager/src/LocalizationManager.cpp index 4151908e8e..ac2bbbebfe 100644 --- a/localization_manager/src/LocalizationManager.cpp +++ b/localization_manager/src/LocalizationManager.cpp @@ -20,295 +20,290 @@ namespace localization_manager { -// Initialize static values -const std::unordered_set LocalizationManager::LIDAR_FAILURE_STRINGS({"One LIDAR Failed", "Both LIDARS Failed"}); - -LocalizationManager::LocalizationManager(PosePublisher pose_pub, - StatePublisher state_pub, ManagedInitialPosePublisher initialpose_pub, - const LocalizationManagerConfig& config, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, - std::unique_ptr timer_factory) -: pose_pub_(pose_pub) -, state_pub_(state_pub) -, initialpose_pub_(initialpose_pub) -, config_(config) -, logger_(logger) -, timer_factory_(std::move(timer_factory)) -, transition_table_(static_cast(config_.localization_mode)) + // Initialize static values + const std::unordered_set LocalizationManager::LIDAR_FAILURE_STRINGS({"One LIDAR Failed", "Both LIDARS Failed"}); -{ - - transition_table_.setTransitionCallback(std::bind(&LocalizationManager::stateTransitionCallback, this, - std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - timer_clock_type_ = timer_factory_->now().get_clock_type(); - -} - -double LocalizationManager::computeFreq(const rclcpp::Time& old_stamp, const rclcpp::Time& new_stamp) const -{ - return 1.0 / (new_stamp - old_stamp).seconds(); // Convert delta to frequency (Hz = 1/s) -} - -double LocalizationManager::computeNDTFreq(const rclcpp::Time& new_stamp) -{ - if (!prev_ndt_stamp_) - { // Check if this is the first data point - prev_ndt_stamp_ = new_stamp; - // When no historic data is available force the frequency into the operational range - return config_.ndt_frequency_degraded_threshold * 2; - } + LocalizationManager::LocalizationManager(PosePublisher pose_pub, + StatePublisher state_pub, ManagedInitialPosePublisher initialpose_pub, + const LocalizationManagerConfig &config, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, + std::unique_ptr timer_factory) + : pose_pub_(pose_pub), state_pub_(state_pub), initialpose_pub_(initialpose_pub), config_(config), logger_(logger), timer_factory_(std::move(timer_factory)), transition_table_(static_cast(config_.localization_mode)) - if (new_stamp <= rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_)) { - RCLCPP_ERROR_STREAM(logger_->get_logger(), "LocalizationManager received NDT data out of order. Prev stamp was " - << prev_ndt_stamp_.get().seconds() << " new stamp is " << new_stamp.seconds()); - // When invalid data is received from NDT force the frequency into the fault range - return config_.ndt_frequency_fault_threshold / 2; - } - return computeFreq(rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_), new_stamp); // Convert delta to frequency (Hz = 1/s) -} -void LocalizationManager::poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, - const autoware_msgs::msg::NDTStat::ConstPtr stats) -{ - double ndt_freq = computeNDTFreq(rclcpp::Time(pose->header.stamp, timer_clock_type_)); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Received pose resulting in frequency value of " << ndt_freq << " with score of " << stats->score); - - if (stats->score >= config_.fitness_score_fault_threshold || ndt_freq <= config_.ndt_frequency_fault_threshold) - { - transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); + transition_table_.setTransitionCallback(std::bind(&LocalizationManager::stateTransitionCallback, this, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + timer_clock_type_ = timer_factory_->now().get_clock_type(); } - else if (stats->score >= config_.fitness_score_degraded_threshold ||ndt_freq <= config_.ndt_frequency_degraded_threshold) + + double LocalizationManager::computeFreq(const rclcpp::Time &old_stamp, const rclcpp::Time &new_stamp) const { - transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); + return 1.0 / (new_stamp - old_stamp).seconds(); // Convert delta to frequency (Hz = 1/s) } - else + + double LocalizationManager::computeNDTFreq(const rclcpp::Time &new_stamp) { - // In GNSS_WITH_NDT_INIT mode, we shouldn't switch back to NDT and only rely on GPS - if (!(static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && - transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX && - lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) + if (!prev_ndt_stamp_) + { // Check if this is the first data point + prev_ndt_stamp_ = new_stamp; + // When no historic data is available force the frequency into the operational range + return config_.ndt_frequency_degraded_threshold * 2; + } + + if (new_stamp <= rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_)) { - transition_table_.signal(LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("localization_manager"), "LocalizationManager received NDT data out of order. Prev stamp was " + << prev_ndt_stamp_.get().seconds() << " new stamp is " << new_stamp.seconds()); + // When invalid data is received from NDT force the frequency into the fault range + return config_.ndt_frequency_fault_threshold / 2; } + return computeFreq(rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_), new_stamp); // Convert delta to frequency (Hz = 1/s) } - const LocalizationState state = transition_table_.getState(); - - if (static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) + void LocalizationManager::poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, + const autoware_msgs::msg::NDTStat::ConstPtr stats) { - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); - if (lidar_init_sequential_timesteps_counter_ < config_.sequential_timesteps_until_gps_operation) + double ndt_freq = computeNDTFreq(rclcpp::Time(pose->header.stamp, timer_clock_type_)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Received pose resulting in frequency value of " << ndt_freq << " with score of " << stats->score); + + if (stats->score >= config_.fitness_score_fault_threshold || ndt_freq <= config_.ndt_frequency_fault_threshold) { - if (is_sequential_) - lidar_init_sequential_timesteps_counter_ ++; - - is_sequential_ = true; + transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); + } + else if (stats->score >= config_.fitness_score_degraded_threshold || ndt_freq <= config_.ndt_frequency_degraded_threshold) + { + transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); } else { - transition_table_.signal(LocalizationSignal::LIDAR_INITIALIZED_SWITCH_TO_GPS); - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Switched to LIDAR_INITIALIZED_SWITCH_TO_GPS at lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ - << ", with new state: " << transition_table_.getState()); + // In GNSS_WITH_NDT_INIT mode, we shouldn't switch back to NDT and only rely on GPS + if (!(static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && + transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX && + lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) + { + transition_table_.signal(LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE); + } } - } - else if (!(static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && - state == LocalizationState::DEGRADED_NO_LIDAR_FIX && - last_raw_gnss_value_ && - lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) // don't reset counter if Lidar initialized and switched to GPS - // as it will reset state to OPERATIONAL - { - is_sequential_ = false; - lidar_init_sequential_timesteps_counter_ = 0; - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); - } - if (state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) { - tf2::Vector3 ndt_translation(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z); + const LocalizationState state = transition_table_.getState(); - tf2::Vector3 gnss_translation(last_raw_gnss_value_->pose.position.x, last_raw_gnss_value_->pose.position.y, last_raw_gnss_value_->pose.position.z); + if (static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); + if (lidar_init_sequential_timesteps_counter_ < config_.sequential_timesteps_until_gps_operation) + { + if (is_sequential_) + lidar_init_sequential_timesteps_counter_++; + + is_sequential_ = true; + } + else + { + transition_table_.signal(LocalizationSignal::LIDAR_INITIALIZED_SWITCH_TO_GPS); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Switched to LIDAR_INITIALIZED_SWITCH_TO_GPS at lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ + << ", with new state: " << transition_table_.getState()); + } + } + else if (!(static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_NDT_INIT && + state == LocalizationState::DEGRADED_NO_LIDAR_FIX && + last_raw_gnss_value_ && + lidar_init_sequential_timesteps_counter_ >= config_.sequential_timesteps_until_gps_operation)) // don't reset counter if Lidar initialized and switched to GPS + // as it will reset state to OPERATIONAL + { + is_sequential_ = false; + lidar_init_sequential_timesteps_counter_ = 0; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); + } - gnss_offset_ = ndt_translation - gnss_translation; - } + if (state == LocalizationState::OPERATIONAL && last_raw_gnss_value_) + { + tf2::Vector3 ndt_translation(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z); + tf2::Vector3 gnss_translation(last_raw_gnss_value_->pose.position.x, last_raw_gnss_value_->pose.position.y, last_raw_gnss_value_->pose.position.z); - if (state != LocalizationState::DEGRADED_NO_LIDAR_FIX) - { - - RCLCPP_DEBUG_STREAM(logger_->get_logger(),"Publishing mixed pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", and state" << state); - current_pose_ = *pose; - } + gnss_offset_ = ndt_translation - gnss_translation; + } - prev_ndt_stamp_ = pose->header.stamp; -} + if (state != LocalizationState::DEGRADED_NO_LIDAR_FIX) + { + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Publishing mixed pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", and state" << state); + current_pose_ = *pose; + } -void LocalizationManager::gnssPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) -{ - last_raw_gnss_value_ = *msg; - // Just like ndt_matching the gnss pose is treated as an initialize signal if the system is not yet intialized - if (transition_table_.getState() == LocalizationState::UNINITIALIZED) { - lidar_init_sequential_timesteps_counter_ = 0; - transition_table_.signal(LocalizationSignal::INITIAL_POSE); - - geometry_msgs::msg::PoseWithCovarianceStamped new_initial_pose; - new_initial_pose.header = msg->header; - new_initial_pose.pose.pose = msg->pose; - - initialpose_pub_(new_initial_pose); - } - - if (transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX) - { - geometry_msgs::msg::PoseStamped corrected_pose = *msg; - if (gnss_offset_) { - corrected_pose.pose.position.x = corrected_pose.pose.position.x + gnss_offset_->x(); - corrected_pose.pose.position.y = corrected_pose.pose.position.y + gnss_offset_->y(); - corrected_pose.pose.position.z = corrected_pose.pose.position.z + gnss_offset_->z(); + prev_ndt_stamp_ = pose->header.stamp; } - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Publishing GNSS pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); - current_pose_ = corrected_pose; - } -} - -void LocalizationManager::initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) -{ - lidar_init_sequential_timesteps_counter_ = 0; - transition_table_.signal(LocalizationSignal::INITIAL_POSE); - current_pose_ = boost::none; // Reset the current pose until a new pose is recieved - initialpose_pub_(*msg); // Forward the initial pose to the rest of the system -} -void LocalizationManager::systemAlertCallback(const carma_msgs::msg::SystemAlert::SharedPtr alert) -{ - if (LIDAR_FAILURE_STRINGS.find(alert->description) != LIDAR_FAILURE_STRINGS.end()) - { - transition_table_.signal(LocalizationSignal::LIDAR_SENSOR_FAILURE); - } -} - -void LocalizationManager::timerCallback(const LocalizationState origin_state) -{ - // If there is already a timer callback in progress or the expected state has changed then return - if (origin_state != transition_table_.getState()) - return; + void LocalizationManager::gnssPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { + last_raw_gnss_value_ = *msg; + // Just like ndt_matching the gnss pose is treated as an initialize signal if the system is not yet intialized + if (transition_table_.getState() == LocalizationState::UNINITIALIZED) + { + lidar_init_sequential_timesteps_counter_ = 0; + transition_table_.signal(LocalizationSignal::INITIAL_POSE); - transition_table_.signal(LocalizationSignal::TIMEOUT); -} + geometry_msgs::msg::PoseWithCovarianceStamped new_initial_pose; + new_initial_pose.header = msg->header; + new_initial_pose.pose.pose = msg->pose; -uint32_t LocalizationManager::nextId() -{ - next_id_++; - return next_id_; -} + initialpose_pub_(new_initial_pose); + } + if (transition_table_.getState() == LocalizationState::DEGRADED_NO_LIDAR_FIX) + { + geometry_msgs::msg::PoseStamped corrected_pose = *msg; + if (gnss_offset_) + { + corrected_pose.pose.position.x = corrected_pose.pose.position.x + gnss_offset_->x(); + corrected_pose.pose.position.y = corrected_pose.pose.position.y + gnss_offset_->y(); + corrected_pose.pose.position.z = corrected_pose.pose.position.z + gnss_offset_->z(); + } + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Publishing GNSS pose with lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_); + current_pose_ = corrected_pose; + } + } -void LocalizationManager::clearTimers() -{ - std::lock_guard guard(mutex_); + void LocalizationManager::initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) + { + lidar_init_sequential_timesteps_counter_ = 0; + transition_table_.signal(LocalizationSignal::INITIAL_POSE); + current_pose_ = boost::none; // Reset the current pose until a new pose is recieved + initialpose_pub_(*msg); // Forward the initial pose to the rest of the system + } - // Clear expired timers - auto it = timers_.begin(); - while(it != timers_.end()) + void LocalizationManager::systemAlertCallback(const carma_msgs::msg::SystemAlert::SharedPtr alert) { - // Check if timer is marked for deletion - if(it->second.second) + if (LIDAR_FAILURE_STRINGS.find(alert->description) != LIDAR_FAILURE_STRINGS.end()) { - // erase() function returns the iterator of the next to last deleted element - it = timers_.erase(it); + transition_table_.signal(LocalizationSignal::LIDAR_SENSOR_FAILURE); } - else{ - it++; + } + + void LocalizationManager::timerCallback(const LocalizationState origin_state) + { + // If there is already a timer callback in progress or the expected state has changed then return + if (origin_state != transition_table_.getState()) + { + return; } + + transition_table_.signal(LocalizationSignal::TIMEOUT); } - -} -void LocalizationManager::stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, - LocalizationSignal signal) -{ - // Mark the expired timers as expired if any - if(current_timer_) + uint32_t LocalizationManager::nextId() { - // NOTE unit testing of current algorithm depends on all timers being one-shot timers. - timers_[current_timer_id_].second = true; + next_id_++; + return next_id_; } - - switch(new_state) + void LocalizationManager::clearTimers() { + std::lock_guard guard(mutex_); + + // Clear expired timers + auto it = timers_.begin(); + while (it != timers_.end()) + { + // Check if timer is marked for deletion + if (it->second.second) + { + // erase() function returns the iterator of the next to last deleted element + it = timers_.erase(it); + } + else + { + it++; + } + } + } + + void LocalizationManager::stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, + LocalizationSignal signal) + { + RCLCPP_INFO_STREAM(rclcpp::get_logger("localization_manager"), "State transition from " << prev_state << " to " << new_state << " with signal " << signal); + // Mark the expired timers as expired if any + if (current_timer_) + { + // NOTE unit testing of current algorithm depends on all timers being one-shot timers. + timers_[current_timer_id_].second = true; + } + + switch (new_state) + { case LocalizationState::INITIALIZING: gnss_offset_ = boost::none; prev_ndt_stamp_ = boost::none; current_timer_id_ = nextId(); - current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.auto_initialization_timeout * 1e6), - std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); + current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.auto_initialization_timeout * 1e6), + std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); - - timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id + timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id break; case LocalizationState::DEGRADED_NO_LIDAR_FIX: + current_timer_id_ = nextId(); + current_timer_ = timer_factory_->buildTimer(current_timer_id_, rclcpp::Duration(config_.gnss_only_operation_timeout * 1e6), + std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); + + timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id - current_timer_ = timer_factory_->buildTimer(nextId(), rclcpp::Duration(config_.gnss_only_operation_timeout * 1e6), - std::bind(&LocalizationManager::timerCallback, this, new_state), true, true); - - timers_[current_timer_id_] = std::make_pair(std::move(current_timer_), false); // Add start timer to map by Id - break; default: break; + } } -} -void LocalizationManager::posePubTick() -{ - // Clear any expired timers - clearTimers(); - - // Evaluate NDT Frequency if we have started receiving messages - // This check provides protection against excessively long NDT computation times that do not trigger the callback - if (prev_ndt_stamp_) + void LocalizationManager::posePubTick() { - double freq = computeFreq(rclcpp::Time(prev_ndt_stamp_.get(),timer_clock_type_), timer_factory_->now()); - if (freq <= config_.ndt_frequency_fault_threshold) + // Clear any expired timers + clearTimers(); + + // Evaluate NDT Frequency if we have started receiving messages + // This check provides protection against excessively long NDT computation times that do not trigger the callback + if (prev_ndt_stamp_) { - transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); + double freq = computeFreq(rclcpp::Time(prev_ndt_stamp_.get(), timer_clock_type_), timer_factory_->now()); + if (freq <= config_.ndt_frequency_fault_threshold) + { + transition_table_.signal(LocalizationSignal::UNUSABLE_NDT_FREQ_OR_FITNESS_SCORE); + } + else if (freq <= config_.ndt_frequency_degraded_threshold) + { + transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); + } } - else if (freq <= config_.ndt_frequency_degraded_threshold) + + // check if last gnss time stamp is older than threshold and send the corresponding signal + if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration(config_.gnss_data_timeout * 1e6)) { - transition_table_.signal(LocalizationSignal::POOR_NDT_FREQ_OR_FITNESS_SCORE); + transition_table_.signal(LocalizationSignal::GNSS_DATA_TIMEOUT); } - } - // check if last gnss time stamp is older than threshold and send the corresponding signal - if (last_raw_gnss_value_ && timer_factory_->now() - rclcpp::Time(last_raw_gnss_value_->header.stamp, timer_clock_type_) > rclcpp::Duration(config_.gnss_data_timeout * 1e6)) - { - transition_table_.signal(LocalizationSignal::GNSS_DATA_TIMEOUT); + // Used in LocalizerMode::GNSS_WITH_NDT_INIT + // If the state is not Operational with good NDT, or already using GPS only, we reset the counter + if (transition_table_.getState() != LocalizationState::OPERATIONAL && + transition_table_.getState() != LocalizationState::DEGRADED_NO_LIDAR_FIX) + { + lidar_init_sequential_timesteps_counter_ = 0; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); + } + + // Publish current pose message if available + if (current_pose_) + { + pose_pub_(*current_pose_); + } + + // Create and publish status report message + carma_localization_msgs::msg::LocalizationStatusReport msg = stateToMsg(transition_table_.getState(), timer_factory_->now()); + state_pub_(msg); } - // Used in LocalizerMode::GNSS_WITH_NDT_INIT - // If the state is not Operational with good NDT, or already using GPS only, we reset the counter - if (transition_table_.getState() != LocalizationState::OPERATIONAL && - transition_table_.getState() != LocalizationState::DEGRADED_NO_LIDAR_FIX) + LocalizationState LocalizationManager::getState() const { - lidar_init_sequential_timesteps_counter_ = 0; - RCLCPP_DEBUG_STREAM(logger_->get_logger(), "posePubTick: Resetting lidar_init_sequential_timesteps_counter_: " << lidar_init_sequential_timesteps_counter_ << ", with new state: " << transition_table_.getState()); - } - - // Publish current pose message if available - if (current_pose_){ - pose_pub_(*current_pose_); + return transition_table_.getState(); } - // Create and publish status report message - carma_localization_msgs::msg::LocalizationStatusReport msg = stateToMsg(transition_table_.getState(), timer_factory_->now()); - state_pub_(msg); -} - -LocalizationState LocalizationManager::getState() const -{ - return transition_table_.getState(); -} - } \ No newline at end of file diff --git a/localization_manager/src/LocalizationTransitionTable.cpp b/localization_manager/src/LocalizationTransitionTable.cpp index 77190ebfcc..9bda5429af 100644 --- a/localization_manager/src/LocalizationTransitionTable.cpp +++ b/localization_manager/src/LocalizationTransitionTable.cpp @@ -14,241 +14,239 @@ * the License. */ - #include "localization_manager/LocalizationTransitionTable.hpp" - namespace localization_manager { -LocalizationTransitionTable::LocalizationTransitionTable(LocalizerMode mode) : mode_(mode) {} + LocalizationTransitionTable::LocalizationTransitionTable(LocalizerMode mode) : mode_(mode) {} -LocalizationState LocalizationTransitionTable::getState() const -{ - return state_; -} + LocalizationState LocalizationTransitionTable::getState() const + { + return state_; + } -void LocalizationTransitionTable::logDebugSignal(LocalizationSignal signal) const -{ - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("localization_manager"), "LocalizationTransitionTable received unsupported signal of "<< signal <<"while in state"<("fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold); - config_.fitness_score_fault_threshold = declare_parameter("fitness_score_fault_threshold",config_.fitness_score_fault_threshold); + config_.fitness_score_fault_threshold = declare_parameter("fitness_score_fault_threshold", config_.fitness_score_fault_threshold); config_.ndt_frequency_degraded_threshold = declare_parameter("ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold); config_.ndt_frequency_fault_threshold = declare_parameter("ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold); config_.auto_initialization_timeout = declare_parameter("auto_initialization_timeout", config_.auto_initialization_timeout); @@ -40,7 +40,6 @@ namespace localization_manager config_.gnss_data_timeout = declare_parameter("gnss_data_timeout", config_.gnss_data_timeout); config_.localization_mode = declare_parameter("localization_mode", config_.localization_mode); config_.pose_pub_rate = declare_parameter("pose_pub_rate", config_.pose_pub_rate); - } carma_ros2_utils::CallbackReturn Node::handle_on_configure(const rclcpp_lifecycle::State &) @@ -48,9 +47,9 @@ namespace localization_manager // Reset config config_ = LocalizationManagerConfig(); - //Load parameters + // Load parameters get_parameter("fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold); - get_parameter("fitness_score_fault_threshold",config_.fitness_score_fault_threshold); + get_parameter("fitness_score_fault_threshold", config_.fitness_score_fault_threshold); get_parameter("ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold); get_parameter("ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold); get_parameter("auto_initialization_timeout", config_.auto_initialization_timeout); @@ -60,87 +59,85 @@ namespace localization_manager get_parameter("localization_mode", config_.localization_mode); get_parameter("pose_pub_rate", config_.pose_pub_rate); + RCLCPP_INFO_STREAM(rclcpp::get_logger("localization_manager"), "Loaded params: "); + // Initialize worker object manager_.reset(new LocalizationManager(std::bind(&Node::publishPoseStamped, this, std_ph::_1), - std::bind(&Node::publishStatus, this, std_ph::_1), - std::bind(&Node::publishManagedInitialPose, this, std_ph::_1), - config_, - get_node_logging_interface(), - std::make_unique(shared_from_this()) - )); + std::bind(&Node::publishStatus, this, std_ph::_1), + std::bind(&Node::publishManagedInitialPose, this, std_ph::_1), + config_, + get_node_logging_interface(), + std::make_unique(shared_from_this()))); // Setup subscribers - gnss_pose_sub_ = create_subscription("gnss_pose", 5, - std::bind(&LocalizationManager::gnssPoseCallback, manager_.get(), std_ph::_1)); - initialpose_sub_ = create_subscription("initialpose", 1, - std::bind(&LocalizationManager::initialPoseCallback, manager_.get(), std_ph::_1)); + gnss_pose_sub_ = create_subscription("gnss_pose", 5, + std::bind(&LocalizationManager::gnssPoseCallback, manager_.get(), std_ph::_1)); + initialpose_sub_ = create_subscription("initialpose", 1, + std::bind(&LocalizationManager::initialPoseCallback, manager_.get(), std_ph::_1)); // Setup synchronized message_filters subscribers rclcpp::QoS qos(5); ndt_pose_sub_.subscribe(this, "ndt_pose", qos.get_rmw_qos_profile()); - ndt_score_sub_.subscribe(this,"ndt_stat", qos.get_rmw_qos_profile()); + ndt_score_sub_.subscribe(this, "ndt_stat", qos.get_rmw_qos_profile()); pose_stats_synchronizer_ = std::make_shared(ndt_pose_sub_, ndt_score_sub_, 5); pose_stats_synchronizer_->registerCallback(std::bind(&Node::poseAndStatsCallback, this, std_ph::_1, std_ph::_2)); - + // system_alert_topic_ protected member of CarmaLifecycleNode - system_alert_sub_ = create_subscription(system_alert_topic_, 1, - std::bind(&LocalizationManager::systemAlertCallback, manager_.get(), std_ph::_1)); + system_alert_sub_ = create_subscription(system_alert_topic_, 1, + std::bind(&LocalizationManager::systemAlertCallback, manager_.get(), std_ph::_1)); // Setup publishers pose_pub_ = create_publisher("selected_pose", 5); state_pub_ = create_publisher("localization_status", 5); - + // Create a publisher that will send all previously published messages to late-joining subscribers ONLY If the subscriber is transient_local too - rclcpp::PublisherOptions intra_proc_disabled; + rclcpp::PublisherOptions intra_proc_disabled; intra_proc_disabled.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; // Disable intra-process comms for this PublisherOptions object auto pub_qos_transient_local = rclcpp::QoS(rclcpp::KeepLast(1)); // 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. + 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. managed_initial_pose_pub_ = create_publisher("managed_initialpose", pub_qos_transient_local, intra_proc_disabled); - //Setup timer - pose_timer_ = create_timer(get_clock(), std::chrono::milliseconds(int(1/config_.pose_pub_rate * 1000)), - std::bind(&LocalizationManager::posePubTick, manager_.get())); + // Setup timer + pose_timer_ = create_timer(get_clock(), std::chrono::milliseconds(int(1 / config_.pose_pub_rate * 1000)), + std::bind(&LocalizationManager::posePubTick, manager_.get())); // Return success if everything initialized successfully return CallbackReturn::SUCCESS; - } - void Node::publishPoseStamped(const geometry_msgs::msg::PoseStamped& msg) const + void Node::publishPoseStamped(const geometry_msgs::msg::PoseStamped &msg) const { pose_pub_->publish(msg); } - void Node::publishStatus(const carma_localization_msgs::msg::LocalizationStatusReport& msg) const + void Node::publishStatus(const carma_localization_msgs::msg::LocalizationStatusReport &msg) const { state_pub_->publish(msg); } - void Node::publishManagedInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) const + void Node::publishManagedInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped &msg) const { managed_initial_pose_pub_->publish(msg); } void Node::poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, - const autoware_msgs::msg::NDTStat::ConstPtr stats) + const autoware_msgs::msg::NDTStat::ConstPtr stats) { try { manager_->poseAndStatsCallback(pose, stats); } - catch (const std::exception& e) + catch (const std::exception &e) { - RCLCPP_ERROR_STREAM(get_logger(), "Uncaught Exception in localization_manager. Exception: "<< e.what() ); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("localization_manager"), "Uncaught Exception in localization_manager. Exception: " << e.what()); handle_primary_state_exception(e); } - - } } // namespace localization_manager #include "rclcpp_components/register_node_macro.hpp" // Register the component with class_loader -RCLCPP_COMPONENTS_REGISTER_NODE(localization_manager::Node) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(localization_manager::Node) diff --git a/localization_manager/src/main.cpp b/localization_manager/src/main.cpp index 776072d817..815ccb66e7 100644 --- a/localization_manager/src/main.cpp +++ b/localization_manager/src/main.cpp @@ -17,17 +17,17 @@ #include #include "localization_manager/localization_manager_node.hpp" -int main(int argc, char **argv) +int main(int argc, char **argv) { - rclcpp::init(argc, 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(); + auto node = std::make_shared(rclcpp::NodeOptions()); - rclcpp::shutdown(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node->get_node_base_interface()); + executor.spin(); - return 0; + rclcpp::shutdown(); + + return 0; } From ec3a613ca5dd412bafcd65eb19a4031df7d2dfd6 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Mon, 3 Oct 2022 08:25:38 -0700 Subject: [PATCH 110/165] Feature/uc3 (#1919) Description The end result of this PR is that carma-platform is able to behave like UC2 by processing MAP/SPAT sent with a similar structure of UC3 (ignoring the strategy parameters). Fixes and implementations to achieve that is as follows: carma_wm_ctrl: Fixed an issue where carma-platform was not able to process multiple signal_groups in a single entry lane (the linked issue). Updated carma_wm_ctrl's MAP msg offset for intersections with most recent message from carma-street carma_wm: Fixed an issue where carma_wm was not updating actual lanelet objects when processing dynamic SPAT (might be easier to only compare the changes in ROS1 as ROS2 was entirely missing the logic, so it looks big). Matched every carma_wm's ROS1 code with ROS2. removed outdated logic Need to unify workzone logic with intersection manager in world model control #1553 arbitrator: Fixed (temporary) an issue where lci_strategic_plugin's namespace was no correctly picked up by arbitrator lci_tactical_plugin: Fixed light_controlled_intersection_tactical_plugin had a bug from porting to ROS2 lci_strategic_plugin: Fixed all the instances where the logic assumes the signal states are fixed, and handles them appropriately for dynamic states. End result now is that the vehicle can behave like UC2 (estimate its own entry time based on signal states) by processing MAP/SPAT sent with a similar structure of UC3 (ignoring the strategy parameters). Future implementation is needed to fully support UC3, once carma-street is able to send strategy_params. Issue is created here: lci_strategic_plugin should be able to plan for entry time outside available signal states (UC3) #1947 Related Issue #1932 usdot-fhwa-stol/autoware.ai#232 Motivation and Context TSMO UC3 incremental integration testing How Has This Been Tested? Integration tested on Black Pacifica with 3 scenarios: speeding to make the green light slowing down to make the green light cruising at minimum speed to stop at red light, before passing at green Co-authored-by: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Co-authored-by: saina-ramyar Co-authored-by: Kyle Rush Co-authored-by: John Stark Co-authored-by: Jon Smet Co-authored-by: Anish_deva <51463994+adev4a@users.noreply.github.com> Co-authored-by: Jon Smet Co-authored-by: John Stark Co-authored-by: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Co-authored-by: saina-ramyar Co-authored-by: Kyle Rush Co-authored-by: Anish_deva <51463994+adev4a@users.noreply.github.com> --- arbitrator/include/capabilities_interface.tpp | 12 +- .../carma_wm/SignalizedIntersectionManager.h | 3 + carma_wm/src/CARMAWorldModel.cpp | 78 ++-- carma_wm/test/CARMAWorldModelTest.cpp | 130 +++++- carma_wm_ctrl/config/parameters.yaml | 4 +- .../include/carma_wm_ctrl/WMBroadcaster.hpp | 4 +- carma_wm_ctrl/src/WMBroadcaster.cpp | 69 ++- .../include/carma_wm_ros2/CARMAWorldModel.hpp | 15 + .../SignalizedIntersectionManager.hpp | 3 + carma_wm_ros2/src/CARMAWorldModel.cpp | 414 ++++++++++-------- carma_wm_ros2/test/CARMAWorldModelTest.cpp | 49 ++- docker/checkout.bash | 2 +- lci_strategic_plugin/config/parameters.yaml | 2 +- .../lci_strategic_plugin.h | 5 +- .../src/lci_strategic_plugin.cpp | 107 +++-- .../src/lci_strategic_plugin_algo.cpp | 3 + ...ontrolled_intersection_tactical_plugin.cpp | 8 +- 17 files changed, 624 insertions(+), 284 deletions(-) diff --git a/arbitrator/include/capabilities_interface.tpp b/arbitrator/include/capabilities_interface.tpp index a77a1bf1b6..6dd86395a9 100644 --- a/arbitrator/include/capabilities_interface.tpp +++ b/arbitrator/include/capabilities_interface.tpp @@ -35,19 +35,23 @@ namespace arbitrator for (auto i = topics.begin(); i != topics.end(); i++) { - auto sc = nh_->create_client(*i); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "found client: " << *i); + auto topic = *i; //todo revert hardcode, lci_strategic is the last ros1 plugin + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1949 + if (topic == "lci_strategic_plugin/plan_maneuvers") + topic = "/guidance/plugins/lci_strategic_plugin/plan_maneuvers"; + 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(*i, resp.get()); + responses.emplace(topic, resp.get()); } else { - RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "failed...: " << *i); + RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "failed...: " << topic); } } return responses; diff --git a/carma_wm/include/carma_wm/SignalizedIntersectionManager.h b/carma_wm/include/carma_wm/SignalizedIntersectionManager.h index c42b617a82..439b3a1400 100644 --- a/carma_wm/include/carma_wm/SignalizedIntersectionManager.h +++ b/carma_wm/include/carma_wm/SignalizedIntersectionManager.h @@ -159,6 +159,9 @@ class SignalizedIntersectionManager // Traffic signal states and their end_time mappings. std::unordered_map>>> traffic_signal_states_; //[intersection_id][signal_group_id] + // Traffic signal's start_time mappings (must be same size as traffic_signal_states_) + std::unordered_map>> traffic_signal_start_times_; //[intersection_id][signal_group_id] + // Last received signal state from SPAT std::unordered_map>> last_seen_state_; //[intersection_id][signal_group_id] diff --git a/carma_wm/src/CARMAWorldModel.cpp b/carma_wm/src/CARMAWorldModel.cpp index d26958505c..78364bead0 100755 --- a/carma_wm/src/CARMAWorldModel.cpp +++ b/carma_wm/src/CARMAWorldModel.cpp @@ -81,26 +81,7 @@ namespace carma_wm } lanelet::Id CARMAWorldModel::getTrafficSignalId(uint16_t intersection_id, uint8_t signal_group_id) - { - if (!traffic_light_ids_.empty()) - { - // TODO: Old logic that needs be removed when workzone is connected - // open issue: https://github.com/usdot-fhwa-stol/carma-platform/issues/1553 - uint32_t temp = 0; - temp |= intersection_id; - temp = temp << 8; - temp |= signal_group_id; - - if (traffic_light_ids_.find(temp) != traffic_light_ids_.end()) - { - return traffic_light_ids_[temp]; - } - else - { - return lanelet::InvalId; - } - } - + { lanelet::Id inter_id = lanelet::InvalId; lanelet::Id signal_id = lanelet::InvalId; @@ -1348,6 +1329,7 @@ namespace carma_wm if (lanelets_general.empty()) { ROS_WARN_STREAM("There was an error querying lanelet for traffic light with id: " << id); + return nullptr; } auto curr_light_list = lanelets_general[0].regulatoryElementsAs(); @@ -1386,15 +1368,24 @@ namespace carma_wm return false; } + // temporary states to save up to date 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(ros::Time::now().toSec()) > mov_check.first) - { - temp_signal_states.push_back(std::make_pair(mov_check.first, mov_check.second )); - } - + if (lanelet::time::timeFromSec(ros::Time::now().toSec()) < mov_check.first) // filter outdated states + { + 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; @@ -1402,9 +1393,10 @@ namespace carma_wm { 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; } @@ -1451,7 +1443,7 @@ namespace carma_wm ROS_WARN_STREAM("No intersection_state_list in the newly received SPAT msg. Returning..."); return; } - + for (const auto& curr_intersection : spat_msg.intersection_state_list) { for (const auto& current_movement_state : curr_intersection.movement_list) @@ -1475,6 +1467,7 @@ namespace carma_wm { ROS_DEBUG_STREAM("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_states_[curr_intersection.id.id][current_movement_state.signal_group].clear(); + } // all maneuver types in same signal group is currently expected to share signal timing, so only 0th index is used when setting states @@ -1484,26 +1477,40 @@ namespace carma_wm continue; } - if(current_movement_state.movement_event_list.size()>1) // Dynamic Spat Processing with future phases + curr_light->revision_ = curr_intersection.revision; // valid SPAT msg + + if(current_movement_state.movement_event_list.size()>1) // Dynamic Spat Processing with future phases { 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 + 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); - if (!recorded) + + if (!recorded) { - 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_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); + ROS_DEBUG_STREAM("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 { - + ROS_DEBUG_STREAM("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); @@ -1637,8 +1644,11 @@ namespace carma_wm 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]++; ROS_DEBUG_STREAM("Counter now: " << sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] << ", for id: "<< curr_light_id); - } } + + } + } } + } } // namespace carma_wm \ No newline at end of file diff --git a/carma_wm/test/CARMAWorldModelTest.cpp b/carma_wm/test/CARMAWorldModelTest.cpp index 83e2d63159..786e2909f9 100755 --- a/carma_wm/test/CARMAWorldModelTest.cpp +++ b/carma_wm/test/CARMAWorldModelTest.cpp @@ -1294,14 +1294,118 @@ TEST(CARMAWorldModelTest, sampleRoutePoints) TEST(CARMAWorldModelTest, getTrafficSignalId) { CARMAWorldModel cmw; - uint32_t id_bit = 257; - cmw.traffic_light_ids_[id_bit] = 1000; uint16_t intersection_id=1; uint8_t signal_group_id=1; + cmw.sim_.intersection_id_to_regem_id_[intersection_id] = 1001; + cmw.sim_.signal_group_to_traffic_light_id_[signal_group_id] = 1000; EXPECT_EQ(cmw.getTrafficSignalId(intersection_id, signal_group_id), 1000); } +TEST(CARMAWorldModelTest, processSpatFromMsgDynamic) +{ + ros::Time::init(); + CARMAWorldModel cmw; + auto pl1 = carma_wm::getPoint(0, 0, 0); + auto pl2 = carma_wm::getPoint(0, 1, 0); + auto pl3 = carma_wm::getPoint(0, 2, 0); + auto pr1 = carma_wm::getPoint(1, 0, 0); + auto pr2 = carma_wm::getPoint(1, 1, 0); + auto pr3 = carma_wm::getPoint(1, 2, 0); + std::vector left_1 = { pl1, pl2, pl3 }; + std::vector right_1 = { pr1, pr2, pr3 }; + auto ll_1 = carma_wm::getLanelet(left_1, right_1, lanelet::AttributeValueString::SolidDashed,lanelet::AttributeValueString::Dashed); + lanelet::Id traffic_light_id = lanelet::utils::getId(); + lanelet::LineString3d virtual_stop_line(lanelet::utils::getId(), {pl2, pr2}); + // Creat passing control line for solid dashed line + std::shared_ptr traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id, { virtual_stop_line }, { ll_1 }, { ll_1 }))); + traffic_light->revision_ = 0; + ll_1.addRegulatoryElement(traffic_light); + auto map = lanelet::utils::createMap({ ll_1 }, {}); + map->add(traffic_light); + cmw.setMap(std::move(map)); + + // create sample SPAT.msg and fill its entries + cav_msgs::SPAT spat; + cav_msgs::IntersectionState state; + state.id.id = 1; + state.revision = 0; + cav_msgs::MovementState movement; + movement.signal_group = 1; + cav_msgs::MovementEvent event; + cmw.sim_.intersection_id_to_regem_id_[state.id.id] = 1; //dummy lanelet id + cmw.sim_.signal_group_to_traffic_light_id_[movement.signal_group] = traffic_light_id; + + + // call with cycle with 2 future events + ros::Time::setNow(ros::Time(0.0)); + event.event_state.movement_phase_state = 5; + event.timing.start_time = 0; + event.timing.min_end_time = 2; + movement.movement_event_list.push_back(event); + event.event_state.movement_phase_state = 8; + event.timing.start_time = 2; + event.timing.min_end_time = 4; + 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(); + // TEST 1. default + EXPECT_EQ(lanelet::time::durationFromSec(0), lights1[0]->fixed_cycle_duration); //since it is dynamic, fixed_cycle_duration is 0 + ASSERT_EQ(lights1[0]->recorded_time_stamps.size(), 2); + double start_time = lanelet::time::toSec(lights1[0]->recorded_start_time_stamps.front()); + double end_time = lanelet::time::toSec(lights1[0]->recorded_time_stamps.back().first); + EXPECT_NEAR(end_time - start_time, movement.movement_event_list.back().timing.min_end_time - + movement.movement_event_list.front().timing.start_time, 0.001); + + // TEST 2. call again with same msg + cmw.processSpatFromMsg(spat); + lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); + + // everything should be same + EXPECT_EQ(lanelet::time::durationFromSec(0), lights1[0]->fixed_cycle_duration); //since it is dynamic, fixed_cycle_duration is 0 + ASSERT_EQ(lights1[0]->recorded_time_stamps.size(), 2); + start_time = lanelet::time::toSec(lights1[0]->recorded_start_time_stamps.front()); + end_time = lanelet::time::toSec(lights1[0]->recorded_time_stamps.back().first); + EXPECT_NEAR(end_time - start_time, movement.movement_event_list.back().timing.min_end_time - + movement.movement_event_list.front().timing.start_time, 0.001); + + // TEST 3. call again with same cycle but different msg + ros::Time::setNow(ros::Time(4.1)); + movement.movement_event_list = {}; + state.movement_list = {}; + spat.intersection_state_list = {}; + + event.event_state.movement_phase_state = 5; + event.timing.start_time = 4; + event.timing.min_end_time = 6; + movement.movement_event_list.push_back(event); + event.event_state.movement_phase_state = 8; + event.timing.start_time = 6; + event.timing.min_end_time = 8; + 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(); + + // everything should still be same + EXPECT_EQ(lanelet::time::durationFromSec(0), lights1[0]->fixed_cycle_duration); //since it is dynamic, fixed_cycle_duration is 0 + ASSERT_EQ(lights1[0]->recorded_time_stamps.size(), 2); + start_time = lanelet::time::toSec(lights1[0]->recorded_start_time_stamps.front()); + end_time = lanelet::time::toSec(lights1[0]->recorded_time_stamps.back().first); + EXPECT_NEAR(end_time - start_time, movement.movement_event_list.back().timing.min_end_time - + movement.movement_event_list.front().timing.start_time, 0.001); + + // TEST 4. check if signalized_manager is being updated correctly + ASSERT_EQ(cmw.sim_.traffic_signal_states_[1][1].size(), 2); + +} + TEST(CARMAWorldModelTest, processSpatFromMsg) { CARMAWorldModel cmw; @@ -1323,8 +1427,12 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) auto map = lanelet::utils::createMap({ ll_1 }, {}); map->add(traffic_light); cmw.setMap(std::move(map)); - uint32_t id_bit = 257; - cmw.traffic_light_ids_[id_bit] = traffic_light_id; + + uint16_t intersection_id=1; + uint8_t signal_group_id=1; + cmw.sim_.intersection_id_to_regem_id_[intersection_id] = 1001; + cmw.sim_.signal_group_to_traffic_light_id_[signal_group_id] = traffic_light_id; + // create sample SPAT.msg and fill its entries cav_msgs::SPAT spat; cav_msgs::IntersectionState state; @@ -1552,17 +1660,19 @@ TEST(CARMAWorldModelTest, getIntersectionAlongRoute) TEST(CARMAWorldModelTest, checkIfSeenBeforeMovementState) { carma_wm::CARMAWorldModel cmw; + ros::Time::init(); + ros::Time::setNow(ros::Time(0.0)); + cmw.sim_.traffic_signal_states_[13][15].push_back(std::make_pair(boost::posix_time::time_from_string("1970-01-01 00:00:01.000"), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); + cmw.sim_.traffic_signal_start_times_[13][15].push_back(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); - cmw.sim_.traffic_signal_states_[13][15].push_back(std::make_pair(boost::posix_time::time_from_string("1970-01-01 00:00:00.000"), lanelet::CarmaTrafficSignalState::STOP_AND_REMAIN)); - - boost::posix_time::ptime min_end_time_dynamic = boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); + boost::posix_time::ptime min_end_time_dynamic = boost::posix_time::time_from_string("1970-01-01 00:00:01.000"); 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=boost::posix_time::time_from_string("1970-01-01 00:00:00.000"); + min_end_time_dynamic=boost::posix_time::time_from_string("1970-01-01 00:00:01.000"); received_state_dynamic= lanelet::CarmaTrafficSignalState::PROTECTED_CLEARANCE; mov_id=13; mov_signal_group=15; @@ -1578,9 +1688,9 @@ TEST(CARMAWorldModelTest, minEndTimeConverterMinuteOfYear) double moy=1; ros::Time::setNow(ros::Time(1)); - ASSERT_EQ(cmw.min_end_time_converter_minute_of_year(min_end_time,moy_exists,moy), boost::posix_time::time_from_string("1970-01-01 00:00:01.000")); + ASSERT_EQ(cmw.min_end_time_converter_minute_of_year(min_end_time,moy_exists,moy), boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); - min_end_time=boost::posix_time::time_from_string("1970-01-01 00:00:01.000"); + min_end_time=boost::posix_time::time_from_string("1970-01-01 01:00:01.000"); moy_exists=1; ros::Time::setNow(ros::Time(3601)); diff --git a/carma_wm_ctrl/config/parameters.yaml b/carma_wm_ctrl/config/parameters.yaml index 5d54aaf0af..b78e9cf0b5 100644 --- a/carma_wm_ctrl/config/parameters.yaml +++ b/carma_wm_ctrl/config/parameters.yaml @@ -7,7 +7,7 @@ max_lane_width: 4.0 traffic_control_request_period: 3.0 #List of Int: Every element corresponds to intersection_id of every two elements (x,y) in intersection_coord_correction (id must be [0, +65535] ranges) -intersection_ids_for_correction: [9945, 9709] +intersection_ids_for_correction: [9945, 9001] #List of Double: Every 2 element describes coordinate correction [delta_x, delta_y] for each intersection_id in intersection_ids_for_correction in same order -intersection_coord_correction: [2.5, -2.5, -0.5, -1.0] \ No newline at end of file +intersection_coord_correction: [0.0, -1.5, -0.5, -1.0] diff --git a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp index 856b449421..ac84cff739 100644 --- a/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp +++ b/carma_wm_ctrl/include/carma_wm_ctrl/WMBroadcaster.hpp @@ -416,6 +416,7 @@ class WMBroadcaster void removeGeofenceHelper(std::shared_ptr gf_ptr) const; void addGeofenceHelper(std::shared_ptr gf_ptr); bool shouldChangeControlLine(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr gf_ptr) const; + bool shouldChangeTrafficSignal(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr sim) const; void addPassingControlLineFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01, const std::vector& affected_llts) const; void addScheduleFromMsg(std::shared_ptr gf_ptr, const carma_v2x_msgs::msg::TrafficControlMessageV01& msg_v01); void scheduleGeofence(std::shared_ptr gf_ptr_list); @@ -459,7 +460,8 @@ class WMBroadcaster size_t update_count_ = -1; // Records the total number of sent map updates. Used as the set value for update.seq_id - carma_wm::SignalizedIntersectionManager sim_; + std::shared_ptr sim_; + enum class AcknowledgementStatus { ACKNOWLEDGED = 1, REJECTED = 2 diff --git a/carma_wm_ctrl/src/WMBroadcaster.cpp b/carma_wm_ctrl/src/WMBroadcaster.cpp index 7bf87922da..745b76dc62 100644 --- a/carma_wm_ctrl/src/WMBroadcaster.cpp +++ b/carma_wm_ctrl/src/WMBroadcaster.cpp @@ -220,11 +220,11 @@ std::vector> WMBroadcaster::geofenceFromMapMsg(std::sh std::vector> intersections; std::vector> traffic_signals; - auto sim_copy = sim_; + auto sim_copy = std::make_shared(*sim_); - sim_.createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, current_map_, current_routing_graph_); + sim_->createIntersectionFromMapMsg(intersections, traffic_signals, map_msg, current_map_, current_routing_graph_); - if (sim_ == sim_copy) // if no change + if (*sim_ == *sim_copy) // if no change { RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ctrl"), ">>> Detected no change from previous, ignoring duplicate message! with gf id: " << gf_ptr->id_); return {}; @@ -1039,13 +1039,13 @@ void WMBroadcaster::externalMapMsgCallback(carma_v2x_msgs::msg::MapData::UniqueP // check if we have seen this message already bool up_to_date = false; - if (sim_.intersection_id_to_regem_id_.size() == map_msg->intersections.size()) + if (sim_->intersection_id_to_regem_id_.size() == map_msg->intersections.size()) { up_to_date = true; // check id of the intersection only for (auto intersection : map_msg->intersections) { - if (sim_.intersection_id_to_regem_id_.find(intersection.id.id) == sim_.intersection_id_to_regem_id_.end()) + if (sim_->intersection_id_to_regem_id_.find(intersection.id.id) == sim_->intersection_id_to_regem_id_.end()) { up_to_date = false; break; @@ -1198,14 +1198,16 @@ void WMBroadcaster::scheduleGeofence(std::shared_ptr gf void WMBroadcaster::geoReferenceCallback(std_msgs::msg::String::UniquePtr geo_ref) { std::lock_guard guard(map_mutex_); - sim_.setTargetFrame(geo_ref->data); + sim_->setTargetFrame(geo_ref->data); base_map_georef_ = geo_ref->data; } void WMBroadcaster::setMaxLaneWidth(double max_lane_width) { + sim_ = std::make_shared(); + max_lane_width_ = max_lane_width; - sim_.setMaxLaneWidth(max_lane_width_); + sim_->setMaxLaneWidth(max_lane_width_); } void WMBroadcaster::setIntersectionCoordCorrection(const std::vector& intersection_ids_for_correction, const std::vector& intersection_correction) @@ -1217,9 +1219,10 @@ void WMBroadcaster::setIntersectionCoordCorrection(const std::vector& i for (auto i = 0; i < intersection_correction.size(); i = i + 2) { - sim_.intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].first = intersection_correction[i]; //x - sim_.intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].second = intersection_correction[i + 1]; //y + sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].first = intersection_correction[i]; //x + sim_->intersection_coord_correction_[(uint16_t)intersection_ids_for_correction[i/2]].second = intersection_correction[i + 1]; //y } + } void WMBroadcaster::setConfigSpeedLimit(double cL) @@ -1374,17 +1377,53 @@ bool WMBroadcaster::shouldChangeControlLine(const lanelet::ConstLaneletOrArea& e return should_change_pcl; } -void WMBroadcaster::addRegulatoryComponent(std::shared_ptr gf_ptr) const +/*! + * \brief This is a helper function that returns true if signal in the lanelet should be changed according to the records of signalizer intersection manager + Used in managing multiple signal_groups in a single entry lanelet for example + * \param el The LaneletOrArea that houses the regem + * \param regem The regulatoryElement that needs to be checked + * \param sim The signalized intersection manager that has records regems and corresponding lanelets + * NOTE: Currently this function only works on lanelets. It returns true if the regem is not CarmaTrafficSignal or if the signal should be changed. + */ +bool WMBroadcaster::shouldChangeTrafficSignal(const lanelet::ConstLaneletOrArea& el,const lanelet::RegulatoryElementConstPtr& regem, std::shared_ptr sim) const { + // should change if the regem is not a CarmaTrafficSignal, which is not supported by this logic + if (regem->attribute(lanelet::AttributeName::Subtype).value().compare(lanelet::CarmaTrafficSignal::RuleName) != 0 || !el.isLanelet() || !sim_) + { + return true; + } + + lanelet::CarmaTrafficSignalPtr traffic_signal = std::dynamic_pointer_cast(current_map_->regulatoryElementLayer.get(regem->id())); + uint8_t signal_id = 0; + + for (auto it = sim->signal_group_to_traffic_light_id_.begin(); it != sim->signal_group_to_traffic_light_id_.end(); ++it) + { + if (regem->id() == it->second) + { + signal_id = it->first; + } + } + if (signal_id == 0) // doesn't exist in the record + return true; + + if (sim->signal_group_to_entry_lanelet_ids_[signal_id].find(el.id()) != sim->signal_group_to_entry_lanelet_ids_[signal_id].end()) + return false; // signal group's entry lane is still part of the intersection, so don't change + + return true; +} +void WMBroadcaster::addRegulatoryComponent(std::shared_ptr gf_ptr) const +{ // First loop is to save the relation between element and regulatory element // so that we can add back the old one after geofence deactivates for (auto el: gf_ptr->affected_parts_) { for (auto regem : el.regulatoryElements()) { - if (!shouldChangeControlLine(el, regem, gf_ptr)) continue; + if (!shouldChangeControlLine(el, regem, gf_ptr) || + !shouldChangeTrafficSignal(el, regem, sim_)) + continue; if (regem->attribute(lanelet::AttributeName::Subtype).value() == gf_ptr->regulatory_element_->attribute(lanelet::AttributeName::Subtype).value()) { @@ -1523,7 +1562,7 @@ void WMBroadcaster::addGeofence(std::shared_ptr gf_ptr) if (detected_map_msg_signal && updates_to_send.back() == update) // if last update { - send_data->sim_ = sim_; + send_data->sim_ = *sim_; } carma_wm::toBinMsg(send_data, &gf_msg); @@ -2174,9 +2213,9 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() } } - if(isLightFound) + if(isLightFound && !sim_) { - for(auto itr = sim_.signal_group_to_traffic_light_id_.begin(); itr != sim_.signal_group_to_traffic_light_id_.end(); itr++) + for(auto itr = sim_->signal_group_to_traffic_light_id_.begin(); itr != sim_->signal_group_to_traffic_light_id_.end(); itr++) { if(itr->second == traffic_lights.front()->id()) { @@ -2200,7 +2239,7 @@ void WMBroadcaster::updateUpcomingSGIntersectionIds() lanelet::Id intersection_id = intersections.front()->id(); if(intersection_id != lanelet::InvalId) { - for(auto itr = sim_.intersection_id_to_regem_id_.begin(); itr != sim_.intersection_id_to_regem_id_.end(); itr++) + for(auto itr = sim_->intersection_id_to_regem_id_.begin(); itr != sim_->intersection_id_to_regem_id_.end(); itr++) { if(itr->second == intersection_id) { diff --git a/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp b/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp index d18f6b2e0b..5bb42f0ffe 100644 --- a/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp +++ b/carma_wm_ros2/include/carma_wm_ros2/CARMAWorldModel.hpp @@ -130,6 +130,21 @@ class CARMAWorldModel : public WorldModel */ lanelet::Optional> getNearestObjInLane(const lanelet::BasicPoint2d& object_center, const LaneSection& section = LANE_AHEAD) const; + /*! \brief update minimum end time to account for minute of the year + * \param min_end_time minimum end time of the spat movement event list + * \param moy_exists tells weather minute of the year exist or not + * \param moy value of the minute of the year + */ + 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); + /** \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_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp b/carma_wm_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp index f1a1c455c8..e467afe0b3 100644 --- a/carma_wm_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp +++ b/carma_wm_ros2/include/carma_wm_ros2/SignalizedIntersectionManager.hpp @@ -162,6 +162,9 @@ class SignalizedIntersectionManager // Traffic signal states and their end_time mappings. std::unordered_map>>> traffic_signal_states_; //[intersection_id][signal_group_id] +// Traffic signal's start_time mappings (must be same size as traffic_signal_states_) + std::unordered_map>> traffic_signal_start_times_; //[intersection_id][signal_group_id] + // Last received signal state from SPAT std::unordered_map>> last_seen_state_; //[intersection_id][signal_group_id] diff --git a/carma_wm_ros2/src/CARMAWorldModel.cpp b/carma_wm_ros2/src/CARMAWorldModel.cpp index 5d0d1544ea..83781acb88 100644 --- a/carma_wm_ros2/src/CARMAWorldModel.cpp +++ b/carma_wm_ros2/src/CARMAWorldModel.cpp @@ -82,26 +82,7 @@ namespace carma_wm } lanelet::Id CARMAWorldModel::getTrafficSignalId(uint16_t intersection_id, uint8_t signal_group_id) - { - if (!traffic_light_ids_.empty()) - { - // TODO: Old logic that needs be removed when workzone is connected - // open issue: https://github.com/usdot-fhwa-stol/carma-platform/issues/1553 - uint32_t temp = 0; - temp |= intersection_id; - temp = temp << 8; - temp |= signal_group_id; - - if (traffic_light_ids_.find(temp) != traffic_light_ids_.end()) - { - return traffic_light_ids_[temp]; - } - else - { - return lanelet::InvalId; - } - } - + { lanelet::Id inter_id = lanelet::InvalId; lanelet::Id signal_id = lanelet::InvalId; @@ -361,7 +342,7 @@ namespace carma_wm std::vector output; if (!route_) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Route has not yet been loaded"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Route has not yet been loaded"); return output; } @@ -370,7 +351,7 @@ namespace carma_wm if (start_downtrack < 0 || start_downtrack > route_end || end_downtrack < 0 || end_downtrack > route_end || start_downtrack > end_downtrack) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Invalid input downtracks"); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Invalid input downtracks"); return output; } @@ -403,13 +384,13 @@ namespace carma_wm if (!route_) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Route has not yet been loaded"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Route has not yet been loaded"); return boost::none; } if (downtrack < 0 || downtrack > getRouteEndTrackPos().downtrack) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Tried to convert a downtrack of: " << downtrack + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Tried to convert a downtrack of: " << downtrack << " to map point, but it did not fit in route bounds of " << getRouteEndTrackPos().downtrack); return boost::none; @@ -535,7 +516,7 @@ namespace carma_wm if (!semantic_map_) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "First time map is set in carma_wm. Routing graph will be recomputed reguardless of method inputs."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ros2"), "First time map is set in carma_wm. Routing graph will be recomputed reguardless of method inputs."); recompute_routing_graph = true; } @@ -547,7 +528,7 @@ namespace carma_wm if (recompute_routing_graph) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Building routing graph"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Building routing graph"); auto tr = getTrafficRules(participant_type_); @@ -561,13 +542,13 @@ namespace carma_wm lanelet::routing::RoutingGraphUPtr map_graph = lanelet::routing::RoutingGraph::build(*semantic_map_, *traffic_rules); map_routing_graph_ = std::move(map_graph); - RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Done building routing graph"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Done building routing graph"); } } void CARMAWorldModel::setRoutingGraph(LaneletRoutingGraphPtr graph) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Setting the routing graph with user or listener provided graph"); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Setting the routing graph with user or listener provided graph"); map_routing_graph_ = graph; } @@ -1179,13 +1160,13 @@ namespace carma_wm // Check if the map is loaded yet if (!semantic_map_ || semantic_map_->laneletLayer.empty()) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Map is not set or does not contain lanelets"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Map is not set or does not contain lanelets"); return {}; } // Check if the route was loaded yet if (!route_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Route has not yet been loaded"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Route has not yet been loaded"); return {}; } std::vector light_list; @@ -1205,7 +1186,7 @@ namespace carma_wm auto stop_line = light->getStopLine(ll); if (!stop_line) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "No stop line"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ros2"), "No stop line"); continue; } else @@ -1279,13 +1260,13 @@ namespace carma_wm // Check if the map is loaded yet if (!semantic_map_ || semantic_map_->laneletLayer.empty()) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Map is not set or does not contain lanelets"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Map is not set or does not contain lanelets"); return {}; } // Check if the route was loaded yet if (!route_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Route has not yet been loaded"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Route has not yet been loaded"); return {}; } std::vector> intersection_list; @@ -1316,13 +1297,13 @@ namespace carma_wm // Check if the map is loaded yet if (!semantic_map_ || semantic_map_->laneletLayer.empty()) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Map is not set or does not contain lanelets"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Map is not set or does not contain lanelets"); return {}; } // Check if the route was loaded yet if (!route_) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Route has not yet been loaded"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Route has not yet been loaded"); return {}; } std::vector intersection_list; @@ -1349,20 +1330,20 @@ namespace carma_wm } lanelet::CarmaTrafficSignalPtr CARMAWorldModel::getTrafficSignal(const lanelet::Id& id) const - { + { auto general_regem = semantic_map_->regulatoryElementLayer.get(id); auto lanelets_general = semantic_map_->laneletLayer.findUsages(general_regem); if (lanelets_general.empty()) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "There was an error querying lanelet for traffic light with id: " << id); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ros2"), "There was an error querying lanelet for traffic light with id: " << id); } auto curr_light_list = lanelets_general[0].regulatoryElementsAs(); if (curr_light_list.empty()) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "There was an error querying traffic light with id: " << id); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ros2"), "There was an error querying traffic light with id: " << id); return nullptr; } @@ -1379,29 +1360,99 @@ namespace carma_wm if (!curr_light) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Was not able to find traffic signal with id: " << id << ", ignoring..."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Was not able to find traffic signal with id: " << id << ", ignoring..."); return nullptr; } 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) + { + if (moy_exists) //account for minute of the year + { + auto inception_boost(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); // inception of epoch + auto duration_since_inception(lanelet::time::durationFromSec(std::chrono::duration(std::chrono::system_clock::now().time_since_epoch()).count())); + auto curr_time_boost = inception_boost + duration_since_inception; + + int curr_year = curr_time_boost.date().year(); + 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); + + int hours_of_day = curr_minute_stamp_boost.time_of_day().hours(); + int curr_month = curr_minute_stamp_boost.date().month(); + int curr_day = curr_minute_stamp_boost.date().day(); + + auto curr_day_boost(boost::posix_time::time_from_string(std::to_string(curr_year) + "/" + std::to_string(curr_month) + "/" + std::to_string(curr_day) +" 00:00:00.000")); // GMT is the standard + auto curr_hour_boost = curr_day_boost + boost::posix_time::hours(hours_of_day); + + min_end_time += lanelet::time::durationFromSec(lanelet::time::toSec(curr_hour_boost)); + return min_end_time; + } + else + { + return min_end_time; // return unchanged + } + } + void CARMAWorldModel::processSpatFromMsg(const carma_v2x_msgs::msg::SPAT &spat_msg) { if (!semantic_map_) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Map is not set yet."); + RCLCPP_INFO_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Map is not set yet."); return; } if (spat_msg.intersection_state_list.empty()) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "No intersection_state_list in the newly received SPAT msg. Returning..."); + RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm_ros2"), "No intersection_state_list in the newly received SPAT msg. Returning..."); return; } for (const auto& curr_intersection : spat_msg.intersection_state_list) - { + { for (const auto& current_movement_state : curr_intersection.movement_list) { lanelet::Id curr_light_id = getTrafficSignalId(curr_intersection.id.id, current_movement_state.signal_group); @@ -1421,168 +1472,189 @@ namespace carma_wm // reset states if the intersection's geometry changed if (curr_light->revision_ != curr_intersection.revision) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Received a new intersection geometry. intersection_id: " << (int)curr_intersection.id.id << ", and signal_group_id: " << (int)current_movement_state.signal_group); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "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_states_[curr_intersection.id.id][current_movement_state.signal_group].clear(); } // all maneuver types in same signal group is currently expected to share signal timing, so only 0th index is used when setting states if (current_movement_state.movement_event_list.empty()) { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Movement_event_list is empty . intersection_id: " << (int)curr_intersection.id.id << ", and signal_group_id: " << (int)current_movement_state.signal_group); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Movement_event_list is empty . intersection_id: " << (int)curr_intersection.id.id << ", and signal_group_id: " << (int)current_movement_state.signal_group); continue; } - - // 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); - - if (curr_intersection.moy_exists) //account for minute of the year + else { - auto inception_boost(boost::posix_time::time_from_string("1970-01-01 00:00:00.000")); // inception of epoch - auto duration_since_inception(lanelet::time::durationFromSec(rclcpp::Time(0.0,0.0,RCL_SYSTEM_TIME).seconds())); - auto curr_time_boost = inception_boost + duration_since_inception; - - int curr_year = curr_time_boost.date().year(); - 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)curr_intersection.moy); - - int hours_of_day = curr_minute_stamp_boost.time_of_day().hours(); - int curr_month = curr_minute_stamp_boost.date().month(); - int curr_day = curr_minute_stamp_boost.date().day(); - - auto curr_day_boost(boost::posix_time::time_from_string(std::to_string(curr_year) + "/" + std::to_string(curr_month) + "/" + std::to_string(curr_day) +" 00:00:00.000")); // GMT is the standard - auto curr_hour_boost = curr_day_boost + boost::posix_time::hours(hours_of_day); - - min_end_time += lanelet::time::durationFromSec(lanelet::time::toSec(curr_hour_boost)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Movement_event_list size: " << current_movement_state.movement_event_list.size() << " . intersection_id: " << (int)curr_intersection.id.id << ", and signal_group_id: " << (int)current_movement_state.signal_group); } - 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; + curr_light->revision_ = curr_intersection.revision; // valid SPAT msg - //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) + if(current_movement_state.movement_event_list.size()>1) // Dynamic Spat Processing with future phases { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "Duplicate as last time! : id: " << curr_light->id() << ", time: " << std::to_string(lanelet::time::toSec(min_end_time))); - continue; - } + 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 - // 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) + 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); + + if (!recorded) + { + 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_ros2"), "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 { - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "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; - } + // 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); - // detected that new state received; therefore, set the last recorded state (not new one received) - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "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=min_end_time_converter_minute_of_year(min_end_time,curr_intersection.moy_exists,curr_intersection.moy); - // update last seen signal state - sim_.last_seen_state_[curr_intersection.id.id][current_movement_state.signal_group] = {min_end_time, received_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::CARMAWorldModel"), "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) + 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) { - // 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::CARMAWorldModel"), "Time_difference new: " << (double)(time_difference.total_milliseconds() / 1000.0)); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Duplicate as last time! : id: " << curr_light->id() << ", time: " << std::to_string(lanelet::time::toSec(min_end_time))); + continue; } - - // 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) + + // 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::CARMAWorldModel"), "Received same cycle info, ignoring : " << std::to_string(lanelet::time::toSec(min_end_time))); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "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; } - // 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) + + // detected that new state received; therefore, set the last recorded state (not new one received) + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "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()); + + // update last seen signal state + sim_.last_seen_state_[curr_intersection.id.id][current_movement_state.signal_group] = {min_end_time, received_state}; + + if (!curr_light->recorded_time_stamps.empty()) { - for ( auto pair : sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group]) + 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_ros2"), "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) { - pair.first = pair.first - time_difference; + // 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_ros2"), "Time_difference new: " << (double)(time_difference.total_milliseconds() / 1000.0)); } - 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::CARMAWorldModel"), "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::CARMAWorldModel"), "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::CARMAWorldModel"), "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)); + // 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; - curr_light->setStates(default_state, curr_intersection.revision); - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::CARMAWorldModel"), "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::CARMAWorldModel"), "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++) + // 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_ros2"), "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_ros2"), "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_ros2"), "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_ros2"), "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_ros2"), "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 light_state = sim_.traffic_signal_states_[curr_intersection.id.id][current_movement_state.signal_group][i + 1].second; + 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_ros2"), "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; + 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_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; - } + 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::CARMAWorldModel"), "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); - } + 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_ros2"), "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::CARMAWorldModel"), "Counter now: " << sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] << ", for id: "<< curr_light_id); + // 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_ros2"), "Counter now: " << sim_.signal_state_counter_[curr_intersection.id.id][current_movement_state.signal_group] << ", for id: "<< curr_light_id); + } } } } diff --git a/carma_wm_ros2/test/CARMAWorldModelTest.cpp b/carma_wm_ros2/test/CARMAWorldModelTest.cpp index 7c307f670a..f8130bcb17 100644 --- a/carma_wm_ros2/test/CARMAWorldModelTest.cpp +++ b/carma_wm_ros2/test/CARMAWorldModelTest.cpp @@ -1071,8 +1071,6 @@ TEST(CARMAWorldModelTest, setConfigSpeedLimitTest) } - - TEST(CARMAWorldModelTest, pointFromRouteTrackPos) { std::shared_ptr wm = std::make_shared(); @@ -1288,10 +1286,10 @@ TEST(CARMAWorldModelTest, sampleRoutePoints) TEST(CARMAWorldModelTest, getTrafficSignalId) { CARMAWorldModel cmw; - uint32_t id_bit = 257; - cmw.traffic_light_ids_[id_bit] = 1000; uint16_t intersection_id=1; uint8_t signal_group_id=1; + cmw.sim_.intersection_id_to_regem_id_[intersection_id] = 1001; + cmw.sim_.signal_group_to_traffic_light_id_[signal_group_id] = 1000; EXPECT_EQ(cmw.getTrafficSignalId(intersection_id, signal_group_id), 1000); } @@ -1317,8 +1315,12 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) auto map = lanelet::utils::createMap({ ll_1 }, {}); map->add(traffic_light); cmw.setMap(std::move(map)); - uint32_t id_bit = 257; - cmw.traffic_light_ids_[id_bit] = traffic_light_id; + + uint16_t intersection_id=1; + uint8_t signal_group_id=1; + cmw.sim_.intersection_id_to_regem_id_[intersection_id] = 1001; + cmw.sim_.signal_group_to_traffic_light_id_[signal_group_id] = traffic_light_id; + // create sample SPAT.msg and fill its entries carma_v2x_msgs::msg::SPAT spat; carma_v2x_msgs::msg::IntersectionState state; @@ -1389,7 +1391,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("CARMAWorldModelTest"), "Input: Duplicate, so ignore"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "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 @@ -1401,7 +1403,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("CARMAWorldModelTest"), "Input: First cycle set. This is technically new cycle, but this info is not counted towards it due to inconvenience"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Input: First cycle set. This is technically new cycle, but this info is not counted towards it due to inconvenience"); cmw.processSpatFromMsg(spat); lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); // same duration, but counter set to 0 @@ -1414,7 +1416,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("CARMAWorldModelTest"), "Input: New cycle, but cycle duration is same due to shifting"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Input: New cycle, but cycle duration is same due to shifting"); cmw.processSpatFromMsg(spat); lights1 = cmw.getMutableMap()->laneletLayer.get(ll_1.id()).regulatoryElementsAs(); @@ -1426,7 +1428,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("CARMAWorldModelTest"),"Input: New partial cycle, yellow reduced"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"),"Input: New partial 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 @@ -1438,7 +1440,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("CARMAWorldModelTest"), "Input: New partial cycle, green reduced"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "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 @@ -1450,7 +1452,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("CARMAWorldModelTest"), "Input: New full cycle, yellow reduced"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "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 @@ -1462,7 +1464,7 @@ TEST(CARMAWorldModelTest, processSpatFromMsg) movement.movement_event_list[0] = event; state.movement_list[0] = movement; spat.intersection_state_list[0] = state; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("CARMAWorldModelTest"), "Input: New full cycle, red reduced"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm_ros2"), "Input: New full cycle, red 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 @@ -1543,5 +1545,26 @@ 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 66eb8395c8..9c93d2c9af 100755 --- a/docker/checkout.bash +++ b/docker/checkout.bash @@ -70,4 +70,4 @@ echo "" > COLCON_IGNORE cd ../ #rosbridge_suite is a ROS meta-package including all the rosbridge packages. -git clone https://github.com/usdot-fhwa-stol/rosbridge_suite --branch ros2 +git clone https://github.com/usdot-fhwa-stol/rosbridge_suite --branch ros2 \ No newline at end of file diff --git a/lci_strategic_plugin/config/parameters.yaml b/lci_strategic_plugin/config/parameters.yaml index 7a603c535e..7792408bd9 100755 --- a/lci_strategic_plugin/config/parameters.yaml +++ b/lci_strategic_plugin/config/parameters.yaml @@ -42,7 +42,7 @@ carma_streets_update_interval : 1.0 reaction_time : 2.0 # 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 : false +enable_carma_streets_connection : true # Double: Mobility operation rate diff --git a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h index 791e76ea89..5e32c6da61 100755 --- a/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h +++ b/lci_strategic_plugin/include/lci_strategic_plugin/lci_strategic_plugin.h @@ -186,7 +186,8 @@ class LCIStrategicPlugin ////////// VARIABLES /////////// TurnDirection intersection_turn_direction_ = TurnDirection::Straight; - bool approaching_light_controlled_interction_ = false; + bool approaching_light_controlled_interction_ = true; // TODO set to true until carma-street is capable of sending strategy parameters for UC3 + // https://github.com/usdot-fhwa-stol/carma-platform/issues/1947 // CARMA Streets Variakes // timestamp for msg received from carma streets @@ -330,7 +331,7 @@ class LCIStrategicPlugin * * \return A transift maneuver message specifically designed for light controlled intersection tactical plugin */ - cav_msgs::Maneuver composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, double start_speed, + cav_msgs::Maneuver composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, const std::vector& crossed_lanelets, double start_speed, double target_speed, ros::Time start_time, ros::Time end_time, const TrajectoryParams& tsp) const; diff --git a/lci_strategic_plugin/src/lci_strategic_plugin.cpp b/lci_strategic_plugin/src/lci_strategic_plugin.cpp index 8eff872294..12e821775a 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin.cpp @@ -264,7 +264,7 @@ void LCIStrategicPlugin::handleFailureCase(const cav_srvs::PlanManeuversRequest& auto incomplete_traj_params = handleFailureCaseHelper(current_state_speed, intersection_speed_.get(), speed_limit, distance_remaining_to_traffic_light, remaining_time, traffic_light_down_track); - resp.new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, + resp.new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets, current_state_speed, incomplete_traj_params.modified_departure_speed, current_state.stamp, current_state.stamp + ros::Duration(incomplete_traj_params.modified_remaining_time), incomplete_traj_params)); double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track; @@ -302,10 +302,14 @@ void LCIStrategicPlugin::handleCruisingUntilStop(const cav_srvs::PlanManeuversRe new_ts_params.v3_ = new_ts_params.v2_; new_ts_params.a3_ = new_ts_params.a2_; - resp.new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, + // Identify the lanelets which will be crossed by approach maneuvers stopping part + std::vector lane_follow_crossed_lanelets = + getLaneletsBetweenWithException(new_ts_params.x1_, new_ts_params.x2_, true, true); + + resp.new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, lane_follow_crossed_lanelets, current_state_speed, new_ts_params.v2_, current_state.stamp, ros::Time(new_ts_params.t2_), new_ts_params)); - // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver + // Identify the lanelets which will be crossed by approach maneuvers stopping part std::vector case_8_crossed_lanelets = getLaneletsBetweenWithException(new_ts_params.x2_, traffic_light_down_track, true, true); @@ -336,6 +340,10 @@ void LCIStrategicPlugin::handleGreenSignalScenario(const cav_srvs::PlanManeuvers ROS_DEBUG_STREAM("Algo initially successful: New light_arrival_time_by_algo: " << std::to_string(light_arrival_time_by_algo.toSec()) << ", with remaining_time: " << std::to_string(remaining_time)); auto can_make_green_optional = canArriveAtGreenWithCertainty(light_arrival_time_by_algo, traffic_light); + // Identify the lanelets which will be crossed by approach maneuvers lane follow maneuver + std::vector crossed_lanelets = + getLaneletsBetweenWithException(current_state.downtrack, traffic_light_down_track, true, true); + // no change for maneuver if invalid light states if (!can_make_green_optional) return; @@ -344,7 +352,7 @@ void LCIStrategicPlugin::handleGreenSignalScenario(const cav_srvs::PlanManeuvers { ROS_DEBUG_STREAM("HANDLE_SUCCESSFULL: Algorithm successful, and able to make it at green with certainty. Planning traj smooth and intersection transit maneuvers"); - resp.new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, + resp.new_plan.maneuvers.push_back(composeTrajectorySmoothingManeuverMessage(current_state.downtrack, traffic_light_down_track, crossed_lanelets, current_state_speed, ts_params.v3_, current_state.stamp, light_arrival_time_by_algo, ts_params)); double intersection_length = intersection_end_downtrack_.get() - traffic_light_down_track; @@ -598,21 +606,45 @@ void LCIStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversReques ROS_DEBUG_STREAM("earliest_entry_time: " << std::to_string(earliest_entry_time.toSec()) << ", with : " << earliest_entry_time - current_state.stamp << " left at: " << std::to_string(current_state.stamp.toSec())); ros::Time nearest_green_entry_time; + bool is_entry_time_within_future_events = false; - if (config_.enable_carma_streets_connection ==false || scheduled_enter_time_ == 0 ) + // Following TODO comments are tracked in this issue: https://github.com/usdot-fhwa-stol/carma-platform/issues/1947 + + if (config_.enable_carma_streets_connection ==false /*|| scheduled_enter_time_ == 0 */) // TODO uncomment when carma-street is capable of sending strategy params { - nearest_green_entry_time = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light) - + ros::Duration(0.01); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehcile Estimation + nearest_green_entry_time = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light) + + ros::Duration(0.01); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehicle Estimation + is_entry_time_within_future_events = true; //UC2 } - else if(config_.enable_carma_streets_connection ==true && scheduled_enter_time_ != 0 ) + else if(config_.enable_carma_streets_connection ==true /*&& scheduled_enter_time_ != 0 */) // TODO uncomment when carma-street is capable of sending strategy params { - nearest_green_entry_time = ros::Time(std::max(earliest_entry_time.toSec(), (scheduled_enter_time_)/1000.0)) + ros::Duration(0.01); //Carma Street + //nearest_green_entry_time = ros::Time(std::max(earliest_entry_time.toSec(), (scheduled_enter_time_)/1000.0)) + ros::Duration(0.01); //Carma Street + + // below is temporary logic to test UC3 incrementally. Testing if UC3 will act like UC2 + // check if there is any GREEN state in the light since scheduled_enter_time_ is not sent yet + nearest_green_entry_time = ros::Time::now() + ros::Duration(60*60.0) + ros::Duration(0.01); //Carma Street TESTING TODO revert, should come from street in the future + for (auto pair : traffic_light->recorded_time_stamps) + { + if (pair.second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED && + lanelet::time::timeFromSec(ros::Time::now().toSec()) < pair.first ) //if not outdated + { + nearest_green_entry_time = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light) + + ros::Duration(0.01); + ROS_DEBUG_STREAM("Up to date GREEN signal exists"); + is_entry_time_within_future_events = true; + break; + } + } } - if (!nearest_green_entry_time_cached_) + ROS_DEBUG_STREAM("nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.toSec()) << ", with : " << nearest_green_entry_time - current_state.stamp << " seconds left at: " << std::to_string(current_state.stamp.toSec())); + + if (!nearest_green_entry_time_cached_ && is_entry_time_within_future_events) { ROS_DEBUG_STREAM("Applying green_light_buffer for the first time and caching! nearest_green_entry_time (without buffer):" << std::to_string(nearest_green_entry_time.toSec()) << ", and earliest_entry_time: " << std::to_string(earliest_entry_time.toSec())); // save first calculated nearest_green_entry_time + buffer to compare against in the future as nearest_green_entry_time changes with earliest_entry_time + + // check if it needs buffer below: ros::Time early_arrival_time_green_et = nearest_green_entry_time - ros::Duration(config_.green_light_time_buffer); @@ -635,21 +667,42 @@ void LCIStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversReques { nearest_green_entry_time_cached_ = nearest_green_entry_time; //don't apply buffer if ET is in green } - else + else //buffer is needed { - auto normal_arrival_state_green_et_optional = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.toSec())); + // below logic stores correct buffered timestamp into nearest_green_entry_time_cached_ to be used later + + ros::Time nearest_green_signal_start_time; + if (traffic_light->fixed_cycle_duration.total_milliseconds()/1000.0 > 1.0) // UC2 + { + ROS_DEBUG_STREAM("UC2 Handling"); + auto normal_arrival_state_green_et_optional = traffic_light->predictState(lanelet::time::timeFromSec(nearest_green_entry_time.toSec())); + + if (!validLightState(normal_arrival_state_green_et_optional, nearest_green_entry_time)) + { + ROS_ERROR_STREAM("Unable to resolve give signal..."); + return; + } - if (!validLightState(normal_arrival_state_green_et_optional, nearest_green_entry_time)) + ROS_DEBUG_STREAM("normal_arrival_signal_end_time: " << std::to_string(lanelet::time::toSec(normal_arrival_state_green_et_optional.get().first))); + + // nearest_green_signal_start_time = normal_arrival_signal_end_time (green guaranteed) - green_signal_duration + nearest_green_signal_start_time = ros::Time(lanelet::time::toSec(normal_arrival_state_green_et_optional.get().first - traffic_light->signal_durations[lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED])); + } + else // UC3 { - ROS_ERROR_STREAM("Unable to resolve give signal..."); - return; + ROS_DEBUG_STREAM("UC3 Handling"); + + for (size_t i = 0; i < traffic_light->recorded_start_time_stamps.size(); i++) + { + if (traffic_light->recorded_time_stamps[i].second == lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED && + lanelet::time::timeFromSec(ros::Time::now().toSec()) < traffic_light->recorded_time_stamps[i].first ) //if not outdated + { + nearest_green_signal_start_time = ros::Time(lanelet::time::toSec(traffic_light->recorded_start_time_stamps[i])); // by using this, we are assuming the ET given by streets falls into this green signal time + break; + } + } } - ROS_DEBUG_STREAM("normal_arrival_signal_end_time: " << std::to_string(lanelet::time::toSec(normal_arrival_state_green_et_optional.get().first))); - - // nearest_green_signal_start_time = normal_arrival_signal_end_time (green guaranteed) - green_signal_duration - ros::Time nearest_green_signal_start_time = ros::Time(lanelet::time::toSec(normal_arrival_state_green_et_optional.get().first - traffic_light->signal_durations[lanelet::CarmaTrafficSignalState::PROTECTED_MOVEMENT_ALLOWED])); - if (early_arrival_time_green_et.toSec() - nearest_green_signal_start_time.toSec() < config_.green_light_time_buffer) { nearest_green_entry_time_cached_ = nearest_green_signal_start_time + ros::Duration(config_.green_light_time_buffer); @@ -669,10 +722,11 @@ void LCIStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversReques nearest_green_entry_time = ros::Time(std::max(nearest_green_entry_time.toSec(), nearest_green_entry_time_cached_.get().toSec())); } - if (nearest_green_entry_time >= nearest_green_entry_time_cached_.get()) + if (nearest_green_entry_time > nearest_green_entry_time_cached_.get()) { ROS_DEBUG_STREAM("Earliest entry time has gone past the cached green light. nearest_green_entry_time_cached_:" << std::to_string(nearest_green_entry_time_cached_.get().toSec()) << ", and earliest_entry_time: " << std::to_string(earliest_entry_time.toSec())); } + ROS_DEBUG_STREAM("Final nearest_green_entry_time: " << std::to_string(nearest_green_entry_time.toSec())); double remaining_time = nearest_green_entry_time.toSec() - current_state.stamp.toSec(); @@ -754,7 +808,7 @@ void LCIStrategicPlugin::planWhenAPPROACHING(const cav_srvs::PlanManeuversReques if (safe_distance_to_stop > distance_remaining_to_traffic_light) { - ROS_DEBUG_STREAM("No longer within safe distance to stop! Decide to continue stopping or continue into intersection"); //TODO handle stopping or handle failure + ROS_DEBUG_STREAM("No longer within safe distance to stop! Decide to continue stopping or continue into intersection"); if (last_case_num_ != TSCase::STOPPING && last_case_num_ != TSCase::UNAVAILABLE && last_case_num_ != TSCase::CASE_8) //case 1-7 { @@ -989,7 +1043,7 @@ bool LCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav return true; } - bool is_empty_schedule_msg = (scheduled_enter_time_ == 0); + bool is_empty_schedule_msg = false ;// = (scheduled_enter_time_ == 0); TODO https://github.com/usdot-fhwa-stol/carma-platform/issues/1947 if (is_empty_schedule_msg) { resp.new_plan.maneuvers = {}; @@ -1034,7 +1088,6 @@ bool LCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav } } - lanelet::CarmaTrafficSignalPtr nearest_traffic_signal = nullptr; lanelet::ConstLanelet entry_lanelet; @@ -1110,7 +1163,7 @@ bool LCIStrategicPlugin::planManeuverCb(cav_srvs::PlanManeuversRequest& req, cav // We need to evaluate the events so the state transitions can be triggered } -cav_msgs::Maneuver LCIStrategicPlugin::composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, double start_speed, +cav_msgs::Maneuver LCIStrategicPlugin::composeTrajectorySmoothingManeuverMessage(double start_dist, double end_dist, const std::vector& crossed_lanelets, double start_speed, double target_speed, ros::Time start_time, ros::Time end_time, const TrajectoryParams& tsp) const { @@ -1148,6 +1201,10 @@ cav_msgs::Maneuver LCIStrategicPlugin::composeTrajectorySmoothingManeuverMessage maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast(tsp.case_num)); maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(static_cast(tsp.is_algorithm_successful)); + for (auto llt : crossed_lanelets) + { + maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(llt.id())); + } // Start time and end time for maneuver are assigned in updateTimeProgress ROS_INFO_STREAM("Creating TrajectorySmoothingManeuver start dist: " << start_dist << " end dist: " << end_dist diff --git a/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp b/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp index dc32d1ec39..121e2e1f00 100755 --- a/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp +++ b/lci_strategic_plugin/src/lci_strategic_plugin_algo.cpp @@ -90,6 +90,9 @@ ros::Time LCIStrategicPlugin::get_nearest_green_entry_time(const ros::Time& curr if (t <= eet) { double cycle_duration = signal->fixed_cycle_duration.total_milliseconds()/1000.0; + if (cycle_duration < 0.001) //if it is a dynamic traffic signal not fixed + cycle_duration = lanelet::time::toSec(signal->recorded_time_stamps.back().first) - lanelet::time::toSec(signal->recorded_start_time_stamps.front()); + t = t + lanelet::time::durationFromSec(std::floor((eet - t).total_milliseconds()/1000.0/cycle_duration) * cycle_duration); //fancy logic was needed to compile curr_pair = signal->predictState(t + boost::posix_time::milliseconds(20)); // select next phase p = curr_pair.get().second; diff --git a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp index f88deda1d0..569ec96d22 100644 --- a/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp +++ b/light_controlled_intersection_tactical_plugin/src/light_controlled_intersection_tactical_plugin.cpp @@ -369,12 +369,10 @@ namespace light_controlled_intersection_tactical_plugin throw std::invalid_argument("No time_to_schedule_entry is provided in float_valued_meta_data"); } - //overwrite maneuver type to use lane follow library function - carma_planning_msgs::msg::Maneuver temp_maneuver = maneuver; - temp_maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Creating Lane Follow Geometry"); std::vector lane_follow_points = basic_autonomy::waypoint_generation::create_lanefollow_geometry(maneuver, starting_downtrack, wm, general_config, detailed_config, visited_lanelets); - points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + points_and_target_speeds.insert(points_and_target_speeds.end(), lane_follow_points.begin(), lane_follow_points.end()); + processed_maneuvers.push_back(maneuver); } else { @@ -385,7 +383,7 @@ namespace light_controlled_intersection_tactical_plugin if(!processed_maneuvers.empty() && processed_maneuvers.back().type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){ points_and_target_speeds = add_lanefollow_buffer(wm, points_and_target_speeds, processed_maneuvers, ending_state_before_buffer, detailed_config); } - + return points_and_target_speeds; } From ee65718b2742b86f27aaa3eee8aa35230bd11096 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 4 Oct 2022 11:31:22 -0700 Subject: [PATCH 111/165] Feature/lcz manager gps offset (#1951) * add offset * fix static * last * integration test * clean * revert --- localization_manager/config/parameters.yaml | 8 +++- .../LocalizationManager.hpp | 7 ++++ .../LocalizationManagerConfig.hpp | 10 ++++- .../LocalizationTypes.hpp | 1 + .../localization_manager_node.hpp | 6 +++ .../src/LocalizationManager.cpp | 14 ++++++- .../src/LocalizationTransitionTable.cpp | 10 ++--- .../src/LocalizationTypes.cpp | 1 + .../src/localization_manager_node.cpp | 40 +++++++++++++++++++ 9 files changed, 89 insertions(+), 8 deletions(-) diff --git a/localization_manager/config/parameters.yaml b/localization_manager/config/parameters.yaml index db71c0cb73..86b0e0496a 100644 --- a/localization_manager/config/parameters.yaml +++ b/localization_manager/config/parameters.yaml @@ -39,4 +39,10 @@ sequential_timesteps_until_gps_operation: 5 # 2 - AUTO select between NDT and GNSS with GNSS timeout # 3 - AUTO select between NDT and GNSS without GNSS timeout # 4 - GNSS only with NDT initialization -localization_mode: 4 \ No newline at end of file +# 5 - GNSS only with fixed offset +localization_mode: 4 + +# Fixed offset to use when GNSS only with fixed offset is being used (Mode 5) +x_offset: 2.8 +y_offset: -2.1 +z_offset: 0.0 diff --git a/localization_manager/include/localization_manager/LocalizationManager.hpp b/localization_manager/include/localization_manager/LocalizationManager.hpp index 4b89ffe7d9..9ae5e64995 100644 --- a/localization_manager/include/localization_manager/LocalizationManager.hpp +++ b/localization_manager/include/localization_manager/LocalizationManager.hpp @@ -126,6 +126,13 @@ namespace localization_manager */ void clearTimers(); + /** + * \brief Set config. + * + * \param config localization manager config + */ + void setConfig(const LocalizationManagerConfig& config); + private: //! The set of strings which mark a lidar failure in a system alert message static const std::unordered_set LIDAR_FAILURE_STRINGS; // Static const container defined in cpp file diff --git a/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp b/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp index 16e62adc14..d9902d5b97 100644 --- a/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp +++ b/localization_manager/include/localization_manager/LocalizationManagerConfig.hpp @@ -47,6 +47,11 @@ namespace localization_manager int localization_mode = static_cast(LocalizerMode::AUTO_WITHOUT_TIMEOUT); //! Selected pose publication rate double pose_pub_rate = 10.0; + //! GPS Offset to apply if that mode is enabled + double x_offset = 2.8; + double y_offset = -2.1; + double z_offset = 0.0; + // Stream operator for this config friend std::ostream &operator<<(std::ostream &output, const LocalizationManagerConfig &c) @@ -62,8 +67,11 @@ namespace localization_manager << "gnss_data_timeout: " << c.gnss_data_timeout << std::endl << "localization_mode: " << static_cast(c.localization_mode) << std::endl << "pose_pub_rate: " << c.pose_pub_rate << std::endl + << "x_offset: " << c.x_offset << std::endl + << "y_offset: " << c.y_offset << std::endl + << "z_offset: " << c.z_offset << std::endl << "}" << std::endl; return output; } }; -} // namespace localization_manager \ No newline at end of file +} // namespace localization_manager diff --git a/localization_manager/include/localization_manager/LocalizationTypes.hpp b/localization_manager/include/localization_manager/LocalizationTypes.hpp index da40455db4..b08e914f10 100644 --- a/localization_manager/include/localization_manager/LocalizationTypes.hpp +++ b/localization_manager/include/localization_manager/LocalizationTypes.hpp @@ -29,6 +29,7 @@ namespace localization_manager AUTO_WITH_TIMEOUT = 2, // NDT operation with support for GPS fallback that will timeout AUTO_WITHOUT_TIMEOUT = 3, // NDT operation with support for GPS fallback that will not timeout GNSS_WITH_NDT_INIT = 4, // GNSS only operation with NDT initialization, switching to GNSS after 5 sequential timesteps of OPERATIONAL NDT + GNSS_WITH_FIXED_OFFSET = 5 // GNSS only operation with fixed offset }; /** diff --git a/localization_manager/include/localization_manager/localization_manager_node.hpp b/localization_manager/include/localization_manager/localization_manager_node.hpp index 5d5be433a9..6c228e764c 100644 --- a/localization_manager/include/localization_manager/localization_manager_node.hpp +++ b/localization_manager/include/localization_manager/localization_manager_node.hpp @@ -103,6 +103,12 @@ namespace localization_manager void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose, const autoware_msgs::msg::NDTStat::ConstPtr stats); + /** + * \brief Callback for dynamic parameter updates + */ + rcl_interfaces::msg::SetParametersResult + parameter_update_callback(const std::vector ¶meters); + //// // Overrides //// diff --git a/localization_manager/src/LocalizationManager.cpp b/localization_manager/src/LocalizationManager.cpp index ac2bbbebfe..6b554e583e 100644 --- a/localization_manager/src/LocalizationManager.cpp +++ b/localization_manager/src/LocalizationManager.cpp @@ -43,6 +43,11 @@ namespace localization_manager return 1.0 / (new_stamp - old_stamp).seconds(); // Convert delta to frequency (Hz = 1/s) } + void LocalizationManager::setConfig(const LocalizationManagerConfig& config) + { + config_ = config; + } + double LocalizationManager::computeNDTFreq(const rclcpp::Time &new_stamp) { if (!prev_ndt_stamp_) @@ -293,7 +298,14 @@ namespace localization_manager // Publish current pose message if available if (current_pose_) { - pose_pub_(*current_pose_); + auto pose_to_publish = *current_pose_; + if (static_cast(config_.localization_mode) == LocalizerMode::GNSS_WITH_FIXED_OFFSET) + { + pose_to_publish.pose.position.x += config_.x_offset; + pose_to_publish.pose.position.y += config_.y_offset; + pose_to_publish.pose.position.z += config_.z_offset; + } + pose_pub_(pose_to_publish); } // Create and publish status report message diff --git a/localization_manager/src/LocalizationTransitionTable.cpp b/localization_manager/src/LocalizationTransitionTable.cpp index 9bda5429af..46cbb7292e 100644 --- a/localization_manager/src/LocalizationTransitionTable.cpp +++ b/localization_manager/src/LocalizationTransitionTable.cpp @@ -52,7 +52,7 @@ namespace localization_manager switch (signal) { case LocalizationSignal::INITIAL_POSE: - if (mode_ == LocalizerMode::GNSS) + if (mode_ == LocalizerMode::GNSS || mode_ == LocalizerMode::GNSS_WITH_FIXED_OFFSET) { setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal); } @@ -171,19 +171,19 @@ namespace localization_manager switch (signal) { case LocalizationSignal::INITIAL_POSE: - if (mode_ != LocalizerMode::GNSS) + if (mode_ != LocalizerMode::GNSS && mode_ != LocalizerMode::GNSS_WITH_FIXED_OFFSET) { setAndLogState(LocalizationState::INITIALIZING, signal); } break; case LocalizationSignal::GOOD_NDT_FREQ_AND_FITNESS_SCORE: - if (mode_ != LocalizerMode::GNSS) + if (mode_ != LocalizerMode::GNSS && mode_ != LocalizerMode::GNSS_WITH_FIXED_OFFSET) { setAndLogState(LocalizationState::OPERATIONAL, signal); } break; case LocalizationSignal::TIMEOUT: - if (mode_ != LocalizerMode::GNSS && mode_ != LocalizerMode::AUTO_WITHOUT_TIMEOUT && mode_ != LocalizerMode::GNSS_WITH_NDT_INIT) + if (mode_ != LocalizerMode::GNSS && mode_ != LocalizerMode::GNSS_WITH_FIXED_OFFSET && mode_ != LocalizerMode::AUTO_WITHOUT_TIMEOUT && mode_ != LocalizerMode::GNSS_WITH_NDT_INIT) { setAndLogState(LocalizationState::AWAIT_MANUAL_INITIALIZATION, signal); } @@ -201,7 +201,7 @@ namespace localization_manager switch (signal) { case LocalizationSignal::INITIAL_POSE: - if (mode_ == LocalizerMode::GNSS) + if (mode_ == LocalizerMode::GNSS || mode_ == LocalizerMode::GNSS_WITH_FIXED_OFFSET) { setAndLogState(LocalizationState::DEGRADED_NO_LIDAR_FIX, signal); } diff --git a/localization_manager/src/LocalizationTypes.cpp b/localization_manager/src/LocalizationTypes.cpp index a238c5648b..6938d627f8 100644 --- a/localization_manager/src/LocalizationTypes.cpp +++ b/localization_manager/src/LocalizationTypes.cpp @@ -25,6 +25,7 @@ namespace localization_manager case LocalizerMode::NDT : os << "NDT"; break; case LocalizerMode::GNSS: os << "GNSS"; break; case LocalizerMode::GNSS_WITH_NDT_INIT: os << "GNSS_WITH_NDT_INIT"; break; + case LocalizerMode::GNSS_WITH_FIXED_OFFSET: os << "GNSS_WITH_FIXED_OFFSET"; break; case LocalizerMode::AUTO_WITH_TIMEOUT : os << "AUTO_WITH_TIMEOUT"; break; case LocalizerMode::AUTO_WITHOUT_TIMEOUT : os << "AUTO_WITHOUT_TIMEOUT"; break; default: os.setstate(std::ios_base::failbit); diff --git a/localization_manager/src/localization_manager_node.cpp b/localization_manager/src/localization_manager_node.cpp index 7f18743657..8e8bc0af7b 100644 --- a/localization_manager/src/localization_manager_node.cpp +++ b/localization_manager/src/localization_manager_node.cpp @@ -40,6 +40,9 @@ namespace localization_manager config_.gnss_data_timeout = declare_parameter("gnss_data_timeout", config_.gnss_data_timeout); config_.localization_mode = declare_parameter("localization_mode", config_.localization_mode); config_.pose_pub_rate = declare_parameter("pose_pub_rate", config_.pose_pub_rate); + config_.x_offset = declare_parameter("x_offset", config_.x_offset); + config_.y_offset = declare_parameter("y_offset", config_.y_offset); + config_.z_offset = declare_parameter("z_offset", config_.z_offset); } carma_ros2_utils::CallbackReturn Node::handle_on_configure(const rclcpp_lifecycle::State &) @@ -58,6 +61,9 @@ namespace localization_manager get_parameter("gnss_data_timeout", config_.gnss_data_timeout); get_parameter("localization_mode", config_.localization_mode); get_parameter("pose_pub_rate", config_.pose_pub_rate); + get_parameter("x_offset", config_.x_offset); + get_parameter("y_offset", config_.y_offset); + get_parameter("z_offset", config_.z_offset); RCLCPP_INFO_STREAM(rclcpp::get_logger("localization_manager"), "Loaded params: "); @@ -69,6 +75,9 @@ namespace localization_manager get_node_logging_interface(), std::make_unique(shared_from_this()))); + // Register runtime parameter update callback + add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1)); + // Setup subscribers gnss_pose_sub_ = create_subscription("gnss_pose", 5, std::bind(&LocalizationManager::gnssPoseCallback, manager_.get(), std_ph::_1)); @@ -117,6 +126,37 @@ namespace localization_manager state_pub_->publish(msg); } + rcl_interfaces::msg::SetParametersResult Node::parameter_update_callback(const std::vector ¶meters) + { + auto error = update_params( + {{"pose_pub_rate", config_.pose_pub_rate}, + {"fitness_score_degraded_threshold", config_.fitness_score_degraded_threshold}, + {"fitness_score_fault_threshold", config_.fitness_score_fault_threshold}, + {"ndt_frequency_degraded_threshold", config_.ndt_frequency_degraded_threshold}, + {"ndt_frequency_fault_threshold", config_.ndt_frequency_fault_threshold}, + {"x_offset", config_.x_offset}, + {"y_offset", config_.y_offset}, + {"z_offset", config_.z_offset}}, parameters); + + auto error_2 = update_params( + {{"auto_initialization_timeout", config_.auto_initialization_timeout}, + {"gnss_only_operation_timeout", config_.gnss_only_operation_timeout}, + {"gnss_data_timeout", config_.gnss_data_timeout}, + {"sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation}, + {"localization_mode", config_.localization_mode}}, parameters); + + rcl_interfaces::msg::SetParametersResult result; + + result.successful = !error && !error_2; + + if (result.successful) + { + manager_->setConfig(config_); + } + + return result; + } + void Node::publishManagedInitialPose(const geometry_msgs::msg::PoseWithCovarianceStamped &msg) const { managed_initial_pose_pub_->publish(msg); From f46d2e523c93f8438191a7013e2af50fe1add812 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Tue, 4 Oct 2022 11:32:33 -0700 Subject: [PATCH 112/165] Fix/uc1 test (#1936) Description This is a assorted fixed related to UC1 regression testing. Mobility path and operation should not be published if the vehicle is not engaged for uc1. bsm id will generate random id from the beginning and uses fixed id if it random_generation is toggled false Mobility path's timestamp should be still up to date when waiting on a stop line Related Issue #1926 BSM ID #1937 Mobility Path timestamp #1938 Mobility Path & Operation should not publish when not engaged Motivation and Context Use case 1 regression testing fixes. How Has This Been Tested? Integration tested on blue lexus and white pacifica Co-authored-by: John Stark Co-authored-by: Michael McConnell --- bsm_generator/config/parameters.yaml | 3 ++ .../bsm_generator/bsm_generator_config.hpp | 2 + .../bsm_generator/bsm_generator_node.hpp | 2 +- .../bsm_generator/bsm_generator_worker.hpp | 7 +++- bsm_generator/src/bsm_generator_node.cpp | 41 ++++++++++++------- bsm_generator/src/bsm_generator_worker.cpp | 25 ++++++----- .../test/test_bsm_generator_worker.cpp | 6 +-- carma/launch/message.launch.py | 3 +- .../mobilitypath_publisher.hpp | 10 +++++ .../src/mobilitypath_publisher.cpp | 15 ++++++- sci_strategic_plugin/config/parameters.yaml | 4 +- .../include/sci_strategic_plugin.hpp | 10 +++++ .../src/sci_strategic_plugin.cpp | 11 ++++- 13 files changed, 103 insertions(+), 36 deletions(-) diff --git a/bsm_generator/config/parameters.yaml b/bsm_generator/config/parameters.yaml index b6e4905f8b..d268837fd7 100644 --- a/bsm_generator/config/parameters.yaml +++ b/bsm_generator/config/parameters.yaml @@ -2,3 +2,6 @@ # Units: Hz bsm_generation_frequency: 10.0 +# Double: BSM id change period +# Units: seconds +bsm_id_change_period: 300.0 \ No newline at end of file diff --git a/bsm_generator/include/bsm_generator/bsm_generator_config.hpp b/bsm_generator/include/bsm_generator/bsm_generator_config.hpp index f726e4b823..5405a6a47f 100644 --- a/bsm_generator/include/bsm_generator/bsm_generator_config.hpp +++ b/bsm_generator/include/bsm_generator/bsm_generator_config.hpp @@ -28,6 +28,7 @@ namespace bsm_generator struct Config { double bsm_generation_frequency = 10.0; // Rate (in Hz) at which a BSM is generated + double bsm_id_change_period = 300.0; // BSM id change period (in sec) bool bsm_id_rotation_enabled = true; // Flag to enable/disable rotation of BSM ID during vehicle operations int bsm_message_id = 0; // Value is converted to a 4 element array of uint8_t where each byte of the parameter becomes one element of the array double vehicle_length = 5.0; // Vehicle length (in meters) @@ -39,6 +40,7 @@ namespace bsm_generator { output << "bsm_generator::Config { " << std::endl << "bsm_generation_frequency: " << c.bsm_generation_frequency << std::endl + << "bsm_id_change_period: " << c.bsm_id_change_period << std::endl << "bsm_id_rotation_enabled: " << c.bsm_id_rotation_enabled << std::endl << "bsm_message_id: " << c.bsm_message_id << std::endl << "vehicle_length: " << c.vehicle_length << std::endl diff --git a/bsm_generator/include/bsm_generator/bsm_generator_node.hpp b/bsm_generator/include/bsm_generator/bsm_generator_node.hpp index d1d3f41f9b..76e1fda993 100644 --- a/bsm_generator/include/bsm_generator/bsm_generator_node.hpp +++ b/bsm_generator/include/bsm_generator/bsm_generator_node.hpp @@ -67,7 +67,7 @@ namespace bsm_generator Config config_; // Worker class - BSMGeneratorWorker worker; + std::shared_ptr worker; // The BSM object that all subscribers make updates to carma_v2x_msgs::msg::BSM bsm_; diff --git a/bsm_generator/include/bsm_generator/bsm_generator_worker.hpp b/bsm_generator/include/bsm_generator/bsm_generator_worker.hpp index 47d896286a..4b2616582e 100644 --- a/bsm_generator/include/bsm_generator/bsm_generator_worker.hpp +++ b/bsm_generator/include/bsm_generator/bsm_generator_worker.hpp @@ -34,7 +34,7 @@ namespace bsm_generator public: /** - * \brief Constructor for BSMGeneratorWorker + * \brief Default Constructor for BSMGeneratorWorker */ BSMGeneratorWorker(); @@ -49,9 +49,10 @@ namespace bsm_generator * \brief Function to obtain the current BSM message ID. The ID is updated to a new random BSM * message ID every 5 minutes. * \param now The current time + * \param secs Id change period in sec * \return The current BSM message ID */ - std::vector getMsgId(const rclcpp::Time now); + std::vector getMsgId(const rclcpp::Time now, double secs); /** * \brief Function to obtain the 'milliseconds' mark of the provided time within the last minute @@ -108,6 +109,8 @@ namespace bsm_generator float getHeadingInRange(const float heading); private: + // Random number generator for BSM id + std::default_random_engine generator_; // BSM message counter value uint8_t msg_count_ {0}; diff --git a/bsm_generator/src/bsm_generator_node.cpp b/bsm_generator/src/bsm_generator_node.cpp index 512c893ad7..19d88284cd 100644 --- a/bsm_generator/src/bsm_generator_node.cpp +++ b/bsm_generator/src/bsm_generator_node.cpp @@ -27,6 +27,7 @@ namespace bsm_generator // Declare parameters config_.bsm_generation_frequency = declare_parameter("bsm_generation_frequency", config_.bsm_generation_frequency); + config_.bsm_id_change_period = declare_parameter("bsm_id_change_period", config_.bsm_id_change_period); config_.bsm_id_rotation_enabled = declare_parameter("bsm_id_rotation_enabled", config_.bsm_id_rotation_enabled); config_.bsm_message_id = declare_parameter("bsm_message_id", config_.bsm_message_id); config_.vehicle_length = declare_parameter("vehicle_length", config_.vehicle_length); @@ -39,6 +40,7 @@ namespace bsm_generator auto error_2 = update_params({{"bsm_message_id", config_.bsm_message_id}}, parameters); auto error_3 = update_params({ {"bsm_generation_frequency", config_.bsm_generation_frequency}, + {"bsm_id_change_period", config_.bsm_id_change_period}, {"vehicle_length", config_.vehicle_length}, {"vehicle_width", config_.vehicle_width} }, parameters); @@ -59,6 +61,7 @@ namespace bsm_generator // Load parameters get_parameter("bsm_generation_frequency", config_.bsm_generation_frequency); + get_parameter("bsm_id_change_period", config_.bsm_id_change_period); get_parameter("bsm_id_rotation_enabled", config_.bsm_id_rotation_enabled); get_parameter("bsm_message_id", config_.bsm_message_id); get_parameter("vehicle_length", config_.vehicle_length); @@ -92,15 +95,11 @@ namespace bsm_generator // Setup publishers bsm_pub_ = create_publisher("bsm_outbound", 5); - // Set the BSM Message ID - for(size_t i = 0; i < 4; ++i) // As the BSM Message ID is a four-element vector, the loop should iterate four times. - { - bsm_message_id_.emplace_back(config_.bsm_message_id >> (8 * i)); - } - // Initialize the generated BSM message initializeBSM(); + worker = std::make_shared(); + // Return success if everthing initialized successfully return CallbackReturn::SUCCESS; } @@ -133,7 +132,7 @@ namespace bsm_generator void BSMGenerator::speedCallback(const geometry_msgs::msg::TwistStamped::UniquePtr msg) { - bsm_.core_data.speed = worker.getSpeedInRange(msg->twist.linear.x); + bsm_.core_data.speed = worker->getSpeedInRange(msg->twist.linear.x); bsm_.core_data.presence_vector = bsm_.core_data.presence_vector | bsm_.core_data.SPEED_AVAILABLE; } @@ -144,25 +143,25 @@ namespace bsm_generator void BSMGenerator::steerWheelAngleCallback(const std_msgs::msg::Float64::UniquePtr msg) { - bsm_.core_data.angle = worker.getSteerWheelAngleInRange(msg->data); + bsm_.core_data.angle = worker->getSteerWheelAngleInRange(msg->data); bsm_.core_data.presence_vector = bsm_.core_data.presence_vector | bsm_.core_data.STEER_WHEEL_ANGLE_AVAILABLE; } void BSMGenerator::accelCallback(const automotive_platform_msgs::msg::VelocityAccelCov::UniquePtr msg) { - bsm_.core_data.accel_set.longitudinal = worker.getLongAccelInRange(msg->accleration); + bsm_.core_data.accel_set.longitudinal = worker->getLongAccelInRange(msg->accleration); bsm_.core_data.accel_set.presence_vector = bsm_.core_data.accel_set.presence_vector | bsm_.core_data.accel_set.ACCELERATION_AVAILABLE; } void BSMGenerator::yawCallback(const sensor_msgs::msg::Imu::UniquePtr msg) { - bsm_.core_data.accel_set.yaw_rate = worker.getYawRateInRange(static_cast(msg->angular_velocity.z)); + bsm_.core_data.accel_set.yaw_rate = worker->getYawRateInRange(static_cast(msg->angular_velocity.z)); bsm_.core_data.accel_set.presence_vector = bsm_.core_data.accel_set.presence_vector | bsm_.core_data.accel_set.YAWRATE_AVAILABLE; } void BSMGenerator::brakeCallback(const std_msgs::msg::Float64::UniquePtr msg) { - bsm_.core_data.brakes.wheel_brakes.brake_applied_status = worker.getBrakeAppliedStatus(msg->data); + bsm_.core_data.brakes.wheel_brakes.brake_applied_status = worker->getBrakeAppliedStatus(msg->data); } void BSMGenerator::poseCallback(const geometry_msgs::msg::PoseStamped::UniquePtr msg) @@ -185,16 +184,28 @@ namespace bsm_generator void BSMGenerator::headingCallback(const gps_msgs::msg::GPSFix::UniquePtr msg) { - bsm_.core_data.heading = worker.getHeadingInRange(static_cast(msg->track)); + bsm_.core_data.heading = worker->getHeadingInRange(static_cast(msg->track)); bsm_.core_data.presence_vector = bsm_.core_data.presence_vector | bsm_.core_data.HEADING_AVAILABLE; } void BSMGenerator::generateBSM() { bsm_.header.stamp = this->now(); - bsm_.core_data.msg_count = worker.getNextMsgCount(); - bsm_.core_data.id = worker.getMsgId( this->now() ); - bsm_.core_data.sec_mark = worker.getSecMark( this->now() ); + bsm_.core_data.msg_count = worker->getNextMsgCount(); + + if (config_.bsm_id_rotation_enabled) + bsm_.core_data.id = worker->getMsgId( this->now(), config_.bsm_id_change_period); + else + { + std::vector id(4); + for(int i = 0; i < id.size(); ++i) + { + id[i] = config_.bsm_message_id >> (8 * i); + } + bsm_.core_data.id = id; + } + + bsm_.core_data.sec_mark = worker->getSecMark( this->now() ); bsm_.core_data.presence_vector = bsm_.core_data.presence_vector | bsm_.core_data.SEC_MARK_AVAILABLE; // currently the accuracy is not available because ndt_matching does not provide accuracy measurement bsm_.core_data.accuracy.presence_vector = 0; diff --git a/bsm_generator/src/bsm_generator_worker.cpp b/bsm_generator/src/bsm_generator_worker.cpp index 935931e847..f1872476b7 100644 --- a/bsm_generator/src/bsm_generator_worker.cpp +++ b/bsm_generator/src/bsm_generator_worker.cpp @@ -15,9 +15,11 @@ */ #include "bsm_generator/bsm_generator_worker.hpp" +#include namespace bsm_generator { + BSMGeneratorWorker::BSMGeneratorWorker() {} uint8_t BSMGeneratorWorker::getNextMsgCount() @@ -31,26 +33,29 @@ namespace bsm_generator return old_msg_count; } - std::vector BSMGeneratorWorker::getMsgId(const rclcpp::Time now) + std::vector BSMGeneratorWorker::getMsgId(const rclcpp::Time now, double secs) { + // need to change ID every designated period + rclcpp::Duration id_timeout(secs * 1e9); + + generator_.seed(std::random_device{}()); // guarantee randomness + std::uniform_int_distribution dis(0,INT_MAX); + + std::vector id(4); + if (first_msg_id_) { last_id_generation_time_ = now; first_msg_id_ = false; + random_id_ = dis(generator_); } - - std::vector id(4); - // need to change ID every 5 mins - rclcpp::Duration id_timeout(60*5, 0); - - std::default_random_engine generator; - std::uniform_int_distribution dis(0,INT_MAX); - if(now - last_id_generation_time_ >= id_timeout) { - random_id_ = dis(generator); + random_id_ = dis(generator_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("bsm_generator"), "Newly generated random id: " << random_id_); last_id_generation_time_ = now; } + for(int i = 0; i < id.size(); ++i) { id[i] = random_id_ >> (8 * i); diff --git a/bsm_generator/test/test_bsm_generator_worker.cpp b/bsm_generator/test/test_bsm_generator_worker.cpp index 50feb8648a..1b0b955e76 100644 --- a/bsm_generator/test/test_bsm_generator_worker.cpp +++ b/bsm_generator/test/test_bsm_generator_worker.cpp @@ -39,11 +39,11 @@ TEST(BSMWorkerTest, testMsgId) { bsm_generator::BSMGeneratorWorker worker; rclcpp::Time time1(10, 0); - std::vector msgId1 = worker.getMsgId(time1); + std::vector msgId1 = worker.getMsgId(time1, 300.0); rclcpp::Time time2(11, 0); - std::vector msgId2 = worker.getMsgId(time2); + std::vector msgId2 = worker.getMsgId(time2, 300.0); rclcpp::Time time3(310, 0); - std::vector msgId3 = worker.getMsgId(time3); + std::vector msgId3 = worker.getMsgId(time3, 300.0); EXPECT_TRUE(msgId1 == msgId2); EXPECT_TRUE(msgId2 != msgId3); } diff --git a/carma/launch/message.launch.py b/carma/launch/message.launch.py index 6c737408ea..d4d4d48442 100644 --- a/carma/launch/message.launch.py +++ b/carma/launch/message.launch.py @@ -89,7 +89,8 @@ def generate_launch_description(): ], remappings=[ ("plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plan_trajectory" ] ), - ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ) + ("georeference", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/map_param_loader/georeference" ] ), + ("mobility_path_msg", [ EnvironmentVariable('CARMA_MSG_NS', default_value=''), "/outgoing_mobility_path" ] ) ], parameters=[ mobilitypath_publisher_param_file, diff --git a/mobilitypath_publisher/include/mobilitypath_publisher/mobilitypath_publisher.hpp b/mobilitypath_publisher/include/mobilitypath_publisher/mobilitypath_publisher.hpp index 4fead9d6b8..8f66a7b569 100644 --- a/mobilitypath_publisher/include/mobilitypath_publisher/mobilitypath_publisher.hpp +++ b/mobilitypath_publisher/include/mobilitypath_publisher/mobilitypath_publisher.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -48,7 +49,10 @@ namespace mobilitypath_publisher carma_ros2_utils::SubPtr traj_sub_; carma_ros2_utils::SubPtr bsm_sub_; carma_ros2_utils::SubPtr georeference_sub_; + carma_ros2_utils::SubPtr guidance_state_sub_; + bool guidance_engaged_ = false; + // Publishers carma_ros2_utils::PubPtr path_pub_; @@ -94,6 +98,12 @@ namespace mobilitypath_publisher */ void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg); + /** + * \brief Callback for the Guidance State + * \param msg Latest GuidanceState message + */ + void guidance_state_cb(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg); + /** * \brief Generates a MobilityHeader to be used for a published MobilityPath message * \param time Time in milliseconds diff --git a/mobilitypath_publisher/src/mobilitypath_publisher.cpp b/mobilitypath_publisher/src/mobilitypath_publisher.cpp index cbdce2433a..d965972483 100644 --- a/mobilitypath_publisher/src/mobilitypath_publisher.cpp +++ b/mobilitypath_publisher/src/mobilitypath_publisher.cpp @@ -63,16 +63,25 @@ namespace mobilitypath_publisher std::bind(&MobilityPathPublication::trajectory_cb, this, std_ph::_1)); bsm_sub_ = create_subscription("bsm_outbound", 1, std::bind(&MobilityPathPublication::bsm_cb, this, std_ph::_1)); + + guidance_state_sub_ = create_subscription("guidance_state", 5, std::bind(&MobilityPathPublication::guidance_state_cb, this, std::placeholders::_1)); + georeference_sub_ = create_subscription("georeference", 1, std::bind(&MobilityPathPublication::georeference_cb, this, std_ph::_1)); // Setup publishers path_pub_ = create_publisher("mobility_path_msg", 5); + // Return success if everthing initialized successfully return CallbackReturn::SUCCESS; } + void MobilityPathPublication::guidance_state_cb(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg) + { + guidance_engaged_ = (msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED); + } + carma_ros2_utils::CallbackReturn MobilityPathPublication::handle_on_activate(const rclcpp_lifecycle::State &prev_state) { // Timer setup @@ -86,7 +95,11 @@ namespace mobilitypath_publisher bool MobilityPathPublication::spin_callback() { - path_pub_->publish(latest_mobility_path_); + // update timestamp of mobilitypath + uint64_t millisecs = get_clock()->now().nanoseconds() / 1000000; + latest_mobility_path_.m_header.timestamp = millisecs; //time in millisecond + if (guidance_engaged_) + path_pub_->publish(latest_mobility_path_); return true; } diff --git a/sci_strategic_plugin/config/parameters.yaml b/sci_strategic_plugin/config/parameters.yaml index b6176a2e3c..1b4811bc58 100644 --- a/sci_strategic_plugin/config/parameters.yaml +++ b/sci_strategic_plugin/config/parameters.yaml @@ -5,7 +5,7 @@ vehicle_decel_limit_multiplier : 0.65 vehicle_accel_limit_multiplier : 0.65 # Double: A buffer before of the stopping location which will still be considered a valid stop. Units in meters -stop_line_buffer : 3.5 +stop_line_buffer : 10 # Double: Update time interval of carma streets. Units in s delta_t : 1.0 @@ -29,4 +29,4 @@ lane_following_plugin_name : stop_controlled_intersection_tactical_plugin stop_and_wait_plugin_name : stop_and_wait_plugin # String: The name of the plugin to use for intersection transit trajectory planning -intersection_transit_plugin_name : intersection_transit_maneuvering \ No newline at end of file +intersection_transit_plugin_name : intersection_transit_maneuvering diff --git a/sci_strategic_plugin/include/sci_strategic_plugin.hpp b/sci_strategic_plugin/include/sci_strategic_plugin.hpp index 115656a92a..324c07714b 100644 --- a/sci_strategic_plugin/include/sci_strategic_plugin.hpp +++ b/sci_strategic_plugin/include/sci_strategic_plugin.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -260,6 +261,12 @@ class SCIStrategicPlugin : public carma_guidance_plugins::StrategicPlugin */ void BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg); + /** + * \brief Callback for the Guidance State + * \param msg Latest GuidanceState message + */ + void guidance_state_cb(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg); + /** * \brief Determine the turn direction at intersection * @@ -315,6 +322,9 @@ class SCIStrategicPlugin : public carma_guidance_plugins::StrategicPlugin carma_ros2_utils::SubPtr current_pose_sub_; carma_ros2_utils::SubPtr bsm_sub_; carma_ros2_utils::PubPtr mobility_operation_pub_; + carma_ros2_utils::SubPtr guidance_state_sub_; + + bool guidance_engaged_ = false; // timer to publish mobility operation message rclcpp::TimerBase::SharedPtr mob_op_pub_timer_; diff --git a/sci_strategic_plugin/src/sci_strategic_plugin.cpp b/sci_strategic_plugin/src/sci_strategic_plugin.cpp index 2f4640ce71..08bd87f8b6 100644 --- a/sci_strategic_plugin/src/sci_strategic_plugin.cpp +++ b/sci_strategic_plugin/src/sci_strategic_plugin.cpp @@ -125,6 +125,10 @@ carma_ros2_utils::CallbackReturn SCIStrategicPlugin::on_configure_plugin() bsm_sub_ = create_subscription("bsm_outbound", 1, std::bind(&SCIStrategicPlugin::BSMCb,this,std_ph::_1)); + // Guidance State subscriber + guidance_state_sub_ = create_subscription("guidance_state", 5, + std::bind(&SCIStrategicPlugin::guidance_state_cb, this, std::placeholders::_1)); + // set world model point form wm listener wm_ = get_world_model(); @@ -219,6 +223,11 @@ void SCIStrategicPlugin::BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg) bsm_sec_mark_ = msg->core_data.sec_mark; } +void SCIStrategicPlugin::guidance_state_cb(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg) +{ + guidance_engaged_ = (msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED); +} + void SCIStrategicPlugin::currentPoseCb(geometry_msgs::msg::PoseStamped::UniquePtr msg) { geometry_msgs::msg::PoseStamped pose_msg = geometry_msgs::msg::PoseStamped(*msg.get()); @@ -809,7 +818,7 @@ carma_v2x_msgs::msg::MobilityOperation SCIStrategicPlugin::generateMobilityOpera void SCIStrategicPlugin::publishMobilityOperation() { - if (approaching_stop_controlled_interction_) + if (approaching_stop_controlled_interction_ && guidance_engaged_) { carma_v2x_msgs::msg::MobilityOperation status_msg = generateMobilityOperation(); mobility_operation_pub_->publish(status_msg); From e5d7c4ae57c6e514de6d8e4a455f172f91668504 Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Wed, 5 Oct 2022 02:35:53 -0700 Subject: [PATCH 113/165] Left over fixes for localization pose node offset (#1952) * pr comment * fix --- localization_manager/config/parameters.yaml | 2 +- localization_manager/src/localization_manager_node.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/localization_manager/config/parameters.yaml b/localization_manager/config/parameters.yaml index 86b0e0496a..b85cbd4053 100644 --- a/localization_manager/config/parameters.yaml +++ b/localization_manager/config/parameters.yaml @@ -42,7 +42,7 @@ sequential_timesteps_until_gps_operation: 5 # 5 - GNSS only with fixed offset localization_mode: 4 -# Fixed offset to use when GNSS only with fixed offset is being used (Mode 5) +# Fixed offset (in meters in map frame) to use when GNSS only with fixed offset is being used (Mode 5) x_offset: 2.8 y_offset: -2.1 z_offset: 0.0 diff --git a/localization_manager/src/localization_manager_node.cpp b/localization_manager/src/localization_manager_node.cpp index 8e8bc0af7b..fc163fb558 100644 --- a/localization_manager/src/localization_manager_node.cpp +++ b/localization_manager/src/localization_manager_node.cpp @@ -142,8 +142,8 @@ namespace localization_manager {{"auto_initialization_timeout", config_.auto_initialization_timeout}, {"gnss_only_operation_timeout", config_.gnss_only_operation_timeout}, {"gnss_data_timeout", config_.gnss_data_timeout}, - {"sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation}, - {"localization_mode", config_.localization_mode}}, parameters); + {"sequential_timesteps_until_gps_operation", config_.sequential_timesteps_until_gps_operation} + }, parameters); rcl_interfaces::msg::SetParametersResult result; From 04475cdef5a3fa866f66f4bc9eb48d3ab299fa4a Mon Sep 17 00:00:00 2001 From: Misheel Bayartsengel Date: Wed, 5 Oct 2022 06:22:37 -0700 Subject: [PATCH 114/165] Fix/mobility viz (#1953) * probable fix * fix * better fix * fix build --- .../src/mobilitypath_visualizer.cpp | 32 ++++++++++++------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp b/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp index 391eaf142f..afca3dbae3 100644 --- a/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp +++ b/mobilitypath_visualizer/src/mobilitypath_visualizer.cpp @@ -165,7 +165,7 @@ namespace mobilitypath_visualizer { visualization_msgs::msg::MarkerArray MobilityPathVisualizer::composeVisualizationMarker(const carma_v2x_msgs::msg::MobilityPath& msg, const MarkerColor& color) { visualization_msgs::msg::MarkerArray output; - + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; @@ -191,18 +191,26 @@ namespace mobilitypath_visualizer { marker.id = 0; geometry_msgs::msg::Point arrow_start; - RCLCPP_DEBUG_STREAM(get_logger(), "ECEF point x: " << curr_location_msg.trajectory.location.ecef_x << ", y:" << curr_location_msg.trajectory.location.ecef_y); - arrow_start = ECEFToMapPoint(curr_location_msg.trajectory.location); //also convert from cm to m - RCLCPP_DEBUG_STREAM(get_logger(), "Map point x: " << arrow_start.x << ", y:" << arrow_start.y); - geometry_msgs::msg::Point arrow_end; - curr_location_msg.trajectory.location.ecef_x += + msg.trajectory.offsets[0].offset_x; - curr_location_msg.trajectory.location.ecef_y += + msg.trajectory.offsets[0].offset_y; - curr_location_msg.trajectory.location.ecef_z += + msg.trajectory.offsets[0].offset_z; - arrow_end = ECEFToMapPoint(curr_location_msg.trajectory.location); //also convert from cm to m - marker.points.push_back(arrow_start); - marker.points.push_back(arrow_end); + if (msg.trajectory.offsets.empty()) + { + marker.action = visualization_msgs::msg::Marker::DELETE; + } + else + { + RCLCPP_DEBUG_STREAM(get_logger(), "ECEF point x: " << curr_location_msg.trajectory.location.ecef_x << ", y:" << curr_location_msg.trajectory.location.ecef_y); + arrow_start = ECEFToMapPoint(curr_location_msg.trajectory.location); //also convert from cm to m + RCLCPP_DEBUG_STREAM(get_logger(), "Map point x: " << arrow_start.x << ", y:" << arrow_start.y); + + curr_location_msg.trajectory.location.ecef_x += + msg.trajectory.offsets[0].offset_x; + curr_location_msg.trajectory.location.ecef_y += + msg.trajectory.offsets[0].offset_y; + curr_location_msg.trajectory.location.ecef_z += + msg.trajectory.offsets[0].offset_z; + arrow_end = ECEFToMapPoint(curr_location_msg.trajectory.location); //also convert from cm to m + + marker.points.push_back(arrow_start); + marker.points.push_back(arrow_end); + } output.markers.push_back(marker); @@ -216,6 +224,8 @@ namespace mobilitypath_visualizer { if (i >= msg.trajectory.offsets.size()) { // If we need to delete previous points marker.action = visualization_msgs::msg::Marker::DELETE; + output.markers.push_back(marker); + continue; } marker.points = {}; From adbf81bbfd9e383ec327793a43d8ec0e9f57ebb9 Mon Sep 17 00:00:00 2001 From: Aswath <32398753+aswath1@users.noreply.github.com> Date: Mon, 10 Oct 2022 13:33:42 -0400 Subject: [PATCH 115/165] unit test (#1956) * unit test * pr * pr --- ...ontrolled_intersection_tactical_plugin.hpp | 6 + .../test/node_test.cpp | 336 +++++++++++++++++- 2 files changed, 337 insertions(+), 5 deletions(-) diff --git a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp index d38cd16fea..977c788eda 100644 --- a/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp +++ b/light_controlled_intersection_tactical_plugin/include/light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin.hpp @@ -186,6 +186,12 @@ namespace light_controlled_intersection_tactical_plugin */ double findSpeedLimit(const lanelet::ConstLanelet& llt, const carma_wm::WorldModelConstPtr &wm) const; + FRIEND_TEST(LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm); + FRIEND_TEST(LCITacticalPluginTest, applyOptimizedTargetSpeedProfile); + FRIEND_TEST(LCITacticalPluginTest, createGeometryProfile); + FRIEND_TEST(LCITacticalPluginTest, planTrajectoryCB); + FRIEND_TEST(LCITacticalPluginTest, setConfig); + public: /*! diff --git a/light_controlled_intersection_tactical_plugin/test/node_test.cpp b/light_controlled_intersection_tactical_plugin/test/node_test.cpp index 37915aace9..2839153c6b 100644 --- a/light_controlled_intersection_tactical_plugin/test/node_test.cpp +++ b/light_controlled_intersection_tactical_plugin/test/node_test.cpp @@ -21,21 +21,347 @@ #include #include "light_controlled_intersection_tactical_plugin/light_controlled_intersection_tactical_plugin_node.hpp" +#include -namespace light_controlled_intersection_transit_plugin +namespace light_controlled_intersection_tactical_plugin { // TODO: The package requires additional unit tests to improve unit test coverage. These unit tests will be created // in a follow-on story. - TEST(LCITacticalPluginTest, apply_accel_cruise_decel_speed_profile_test) + TEST(LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm) { + std::shared_ptr wm = std::make_shared(); + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); + wm->setMap(map); + carma_wm::test::setSpeedLimit(15_mph, wm); + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + rclcpp::NodeOptions options; auto lci_node = std::make_shared(options); - lci_node->configure(); // Call configure state transition - lci_node->activate(); // Call activate state transition + Config config; + config.minimum_speed = 1; + + std::vector points_and_target_speeds; + + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, lci_node->get_node_logging_interface()); + + TrajectoryParams tp; + tp.v1_ = 1.0; + tp.v2_ = 2.0; + tp.v3_ = 2.0; + tp.x1_ = 2.0; + tp.x2_ = 4.0; + tp.x3_ = 5.0; + + EXPECT_THROW(lci_tactical.applyTrajectorySmoothingAlgorithm(wm, points_and_target_speeds, 0, 0, 0, 0, tp), std::invalid_argument); + + PointSpeedPair pair; + pair.point = {1., 1.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 2.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 3.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 4.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 5.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + + lci_tactical.applyTrajectorySmoothingAlgorithm(wm, points_and_target_speeds, 1, 4, 1, 1, tp); + + EXPECT_NEAR(points_and_target_speeds.front().speed, 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.front().point.y(), 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); + + lci_tactical.applyTrajectorySmoothingAlgorithm(wm, points_and_target_speeds, 1, 5, 1, 1, tp); + + EXPECT_NEAR(points_and_target_speeds.front().speed, 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.front().point.y(), 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); + + + points_and_target_speeds = {}; + pair.point = {1., 1.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 2.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 3.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 4.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 5.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + + tp.v1_ = 1.0; + tp.a1_ = 0.5; + tp.v2_ = 2.0; + tp.a2_ = 0.5; + tp.v3_ = 3.0; + tp.a3_ = 0.5; + tp.x1_ = 2.0; + tp.x2_ = 4.0; + tp.x3_ = 5.0; + + lci_tactical.applyTrajectorySmoothingAlgorithm(wm, points_and_target_speeds, 1, 5, 1, 1, tp); + + EXPECT_NEAR(points_and_target_speeds.front().speed, 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, 2.23606, 0.001); + EXPECT_NEAR(points_and_target_speeds.front().point.y(), 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); + + } + + TEST(LCITacticalPluginTest, applyOptimizedTargetSpeedProfile) + { + std::shared_ptr wm = std::make_shared(); + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); + wm->setMap(map); + carma_wm::test::setSpeedLimit(15_mph, wm); + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + + rclcpp::NodeOptions options; + auto lci_node = std::make_shared(options); + Config config; + config.minimum_speed = 1; + + std::vector points_and_target_speeds; + + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, lci_node->get_node_logging_interface()); + + carma_planning_msgs::msg::Maneuver maneuver_msg; + maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver_msg.lane_following_maneuver.parameters.negotiation_type = + carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION; + maneuver_msg.lane_following_maneuver.parameters.presence_vector = + carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA; + maneuver_msg.lane_following_maneuver.start_dist = 1; + maneuver_msg.lane_following_maneuver.start_speed = 1; + maneuver_msg.lane_following_maneuver.end_dist = 4; + maneuver_msg.lane_following_maneuver.end_speed = 1; + + TrajectoryParams tsp; + tsp.v1_ = 1.0; + tsp.v2_ = 2.0; + tsp.v3_ = 2.0; + tsp.x1_ = 2.0; + tsp.x2_ = 4.0; + tsp.x3_ = 5.0; + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a1_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v1_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x1_); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a2_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v2_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x2_); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a3_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v3_); + + maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1); + maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1); + + PointSpeedPair pair; + pair.point = {1., 1.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 2.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 3.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 4.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + pair.point = {1., 5.}; + pair.speed = 99.0; + points_and_target_speeds.push_back(pair); + + EXPECT_THROW(lci_tactical.applyOptimizedTargetSpeedProfile(maneuver_msg, maneuver_msg.lane_following_maneuver.start_speed, points_and_target_speeds), std::invalid_argument); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_); + + lci_tactical.applyOptimizedTargetSpeedProfile(maneuver_msg, maneuver_msg.lane_following_maneuver.start_speed, points_and_target_speeds); + + EXPECT_NEAR(points_and_target_speeds.front().speed, 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.front().point.y(), 1.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().point.y(), 5.0, 0.001); + + } + + TEST(LCITacticalPluginTest, createGeometryProfile) + { + std::shared_ptr wm = std::make_shared(); + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); + wm->setMap(map); + carma_wm::test::setSpeedLimit(15_mph, wm); + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + + rclcpp::NodeOptions options; + auto lci_node = std::make_shared(options); + Config config; + config.minimum_speed = 1; + + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, lci_node->get_node_logging_interface()); + + carma_planning_msgs::msg::Maneuver maneuver_msg; + maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver_msg.lane_following_maneuver.parameters.negotiation_type = + carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION; + maneuver_msg.lane_following_maneuver.parameters.presence_vector = + carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA; + maneuver_msg.lane_following_maneuver.start_dist = 1; + maneuver_msg.lane_following_maneuver.start_speed = 1; + maneuver_msg.lane_following_maneuver.end_dist = 4; + maneuver_msg.lane_following_maneuver.end_speed = 2; + + TrajectoryParams tsp; + tsp.v1_ = 1.0; + tsp.v2_ = 2.0; + tsp.v3_ = 2.0; + tsp.x1_ = 2.0; + tsp.x2_ = 4.0; + tsp.x3_ = 5.0; + + carma_planning_msgs::msg::VehicleState state; + state.x_pos_global = 1.0; + state.y_pos_global = 0.0; + state.longitudinal_vel = 1; + + carma_planning_msgs::msg::VehicleState ending_state; + ending_state.x_pos_global = 1.0; + ending_state.y_pos_global = 5.0; + ending_state.longitudinal_vel = 2.0; + + DetailedTrajConfig wpg_detail_config; + GeneralTrajConfig wpg_general_config; + wpg_general_config.default_downsample_ratio = 1; + + EXPECT_THROW(lci_tactical.createGeometryProfile({maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config), std::invalid_argument); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a1_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v1_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x1_); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a2_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v2_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x2_); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a3_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v3_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_); + + maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1); + maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1); + + maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(1200)); + + EXPECT_THROW(lci_tactical.createGeometryProfile({maneuver_msg, maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config), std::invalid_argument); + + + EXPECT_EQ(lci_tactical.createGeometryProfile({maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config).size(), 7); + + auto points_and_target_speeds = lci_tactical.createGeometryProfile({maneuver_msg}, 1, wm, ending_state, state, wpg_general_config, wpg_detail_config); + + EXPECT_NEAR(points_and_target_speeds.front().speed, 2.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.back().speed, 2.0, 0.001); + EXPECT_NEAR(points_and_target_speeds.front().point.y(), 0.0, 0.001); + } -} // namespace light_controlled_intersection_transit_plugin + TEST(LCITacticalPluginTest, planTrajectoryCB) + { + std::shared_ptr wm = std::make_shared(); + auto map = carma_wm::test::buildGuidanceTestMap(3.7, 10); + wm->setMap(map); + carma_wm::test::setSpeedLimit(15_mph, wm); + carma_wm::test::setRouteByIds({ 1200, 1201, 1202, 1203 }, wm); + + + rclcpp::NodeOptions options; + auto lci_node = std::make_shared(options); + Config config; + config.minimum_speed = 1; + config.default_downsample_ratio = 1; + + auto lci_tactical = LightControlledIntersectionTacticalPlugin(wm, config, lci_node->get_node_logging_interface()); + + carma_planning_msgs::msg::Maneuver maneuver_msg; + maneuver_msg.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; + maneuver_msg.lane_following_maneuver.parameters.negotiation_type = + carma_planning_msgs::msg::ManeuverParameters::NO_NEGOTIATION; + maneuver_msg.lane_following_maneuver.parameters.presence_vector = + carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN | carma_planning_msgs::msg::ManeuverParameters::HAS_FLOAT_META_DATA | carma_planning_msgs::msg::ManeuverParameters::HAS_INT_META_DATA; + maneuver_msg.lane_following_maneuver.start_dist = 1; + maneuver_msg.lane_following_maneuver.start_speed = 1; + maneuver_msg.lane_following_maneuver.end_dist = 11.0; + maneuver_msg.lane_following_maneuver.end_speed = 2; + + TrajectoryParams tsp; + tsp.v1_ = 1.0; + tsp.v2_ = 2.0; + tsp.v3_ = 2.0; + tsp.x1_ = 2.0; + tsp.x2_ = 4.0; + tsp.x3_ = 5.0; + + carma_planning_msgs::msg::VehicleState state; + state.x_pos_global = 1.0; + state.y_pos_global = 0.1; + state.longitudinal_vel = 1; + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a1_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v1_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x1_); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a2_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v2_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x2_); + + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.a3_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.v3_); + maneuver_msg.lane_following_maneuver.parameters.float_valued_meta_data.push_back(tsp.x3_); + + maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1); + maneuver_msg.lane_following_maneuver.parameters.int_valued_meta_data.push_back(1); + maneuver_msg.lane_following_maneuver.parameters.string_valued_meta_data.push_back("signalized"); + + maneuver_msg.lane_following_maneuver.lane_ids.push_back(std::to_string(1200)); + + auto req = std::make_shared(); + auto resp = std::make_shared(); + + req->maneuver_plan.maneuvers.push_back(maneuver_msg); + req->maneuver_index_to_plan = 0; + req->vehicle_state = state; + + lci_tactical.planTrajectoryCB(req, resp); + + EXPECT_NEAR(rclcpp::Time(resp->trajectory_plan.trajectory_points.front().target_time).seconds(), 0.0, 0.001); + EXPECT_NEAR(rclcpp::Time(resp->trajectory_plan.trajectory_points.back().target_time).seconds(), 9.23, 0.1); + EXPECT_NEAR(resp->trajectory_plan.trajectory_points.front().y, 0.1, 0.001); + + } + + +} // namespace light_controlled_intersection_tactical_plugin int main(int argc, char ** argv) From 46715bae76f01cac4455894a96540c480cb9b9cb Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Fri, 14 Oct 2022 12:29:53 -0400 Subject: [PATCH 116/165] Add Doxygen files and GH Action workflow for pushing to usdot-fhwa-stol/documentation (#1931) --- .github/workflows/main.yml | 59 + Doxyfile | 2494 ++++++++++++++++++++++++++++++++++++ docs/mainpage.md | 138 ++ 3 files changed, 2691 insertions(+) create mode 100644 .github/workflows/main.yml create mode 100644 Doxyfile create mode 100644 docs/mainpage.md diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 0000000000..fc2134e6c9 --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,59 @@ +# Copyright (C) 2018-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. + +# Configuration file for GitHub Actions +# CI will report failure if any executed command returns and error status +# Operations performed are as follows +# Build source code Documentation +# Run Doxygen Action +# Deploy documentation static webpage to gh-pages + +name: Doxygen Action + +# Controls when the action will run. Triggers the workflow on push or pull Request events but only for the develop branch +on: + push: + branches: [ develop ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + # This workflow contains a single job called "build" + build: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Install graphvix to support Graph visualization and representing structural information as diagrams of abstract graphs and networks + - name: Install graphviz + run: sudo apt install graphviz && sudo dot -c + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v2 + # Doxygen action for documentation of carma-platform source code by using mattnotmitt github actions workflow + - name: Doxygen Action + uses: mattnotmitt/doxygen-action@v1.9.4 + with: + # Path to Doxyfile + doxyfile-path: "./Doxyfile" # default is ./Doxyfile + # Working directory + working-directory: "." # default is . + - name: Deploy + uses: peaceiris/actions-gh-pages@v3 + with: + deploy_key: ${{ secrets.ACTIONS_DEPLOY_KEY }} + external_repository: usdot-fhwa-stol/documentation + publish_branch: gh-pages + # default: gh-pages + publish_dir: ./docs/html/ + destination_dir: ${{ github.event.repository.name }} diff --git a/Doxyfile b/Doxyfile new file mode 100644 index 0000000000..dffa5396e1 --- /dev/null +++ b/Doxyfile @@ -0,0 +1,2494 @@ +# Doxyfile 1.9.5 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the configuration +# file that follow. The default is UTF-8 which is also the encoding used for all +# text before the first occurrence of this tag. Doxygen uses libiconv (or the +# iconv built into libc) for the transcoding. See +# https://www.gnu.org/software/libiconv/ for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "Carma-platform" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = "v4.2.0" + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication. + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = ./docs/ + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line +# such as +# /*************** +# as being the beginning of a Javadoc-style comment "banner". If set to NO, the +# Javadoc-style will behave just like regular comments and it will not be +# interpreted by doxygen. +# The default value is: NO. + +JAVADOC_BANNER = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# By default Python docstrings are displayed as preformatted text and doxygen's +# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the +# doxygen's special commands can be used and the contents of the docstring +# documentation blocks is shown as doxygen documentation. +# The default value is: YES. + +PYTHON_DOCSTRING = YES + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines (in the resulting output). You can put ^^ in the value part of an +# alias to insert a newline as if a physical newline was in the original file. +# When you need a literal { or } or , in the value part of an alias you have to +# escape them by means of a backslash (\), this can lead to conflicts with the +# commands \{ and \} for these it is advised to use the version @{ and @} or use +# a double escape (\\{ and \\}) + +ALIASES = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice +# sources only. Doxygen will then generate output that is more tailored for that +# language. For instance, namespaces will be presented as modules, types will be +# separated into more groups, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_SLICE = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, JavaScript, +# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, VHDL, +# Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: +# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser +# tries to guess whether the code is fixed or free formatted code, this is the +# default for Fortran type files). For instance to make doxygen treat .inc files +# as Fortran files (default is PHP), and .f files as C (default is Fortran), +# use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. When specifying no_extension you should add +# * to the FILE_PATTERNS. +# +# Note see also the list of default file extension mappings. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See https://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up +# to that level are automatically included in the table of contents, even if +# they do not have an id attribute. +# Note: This feature currently applies only to Markdown headings. +# Minimum value: 0, maximum value: 99, default value: 5. +# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. + +TOC_INCLUDE_HEADINGS = 5 + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use +# during processing. When set to 0 doxygen will based this on the number of +# cores available in the system. You can set it explicitly to a value larger +# than 0 to get more control over the balance between CPU load and processing +# speed. At this moment only the input processing can be done using multiple +# threads. Since this is still an experimental feature the default is set to 1, +# which efficively disables parallel processing. Please report any issues you +# encounter. Generating dot graphs in parallel is controlled by the +# DOT_NUM_THREADS setting. +# Minimum value: 0, maximum value: 32, default value: 1. + +NUM_PROC_THREADS = 1 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = YES + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = YES + +# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual +# methods of a class will be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIV_VIRTUAL = YES + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = YES + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = YES + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = YES + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = YES + +# If this flag is set to YES, the name of an unnamed parameter in a declaration +# will be determined by the corresponding definition. By default unnamed +# parameters remain unnamed in the output. +# The default value is: YES. + +RESOLVE_UNNAMED_PARAMS = YES + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# declarations. If set to NO, these declarations will be included in the +# documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# With the correct setting of option CASE_SENSE_NAMES doxygen will better be +# able to match the capabilities of the underlying filesystem. In case the +# filesystem is case sensitive (i.e. it supports files in the same directory +# whose names only differ in casing), the option must be set to YES to properly +# deal with such files in case they appear in the input. For filesystems that +# are not case sensitive the option should be be set to NO to properly deal with +# output files written for symbols that only differ in casing, such as for two +# classes, one named CLASS and the other named Class, and to also support +# references to files without having to specify the exact matching casing. On +# Windows (including Cygwin) and MacOS, users should typically set this option +# to NO, whereas on Linux or other Unix flavors it should typically be set to +# YES. +# The default value is: system dependent. + +CASE_SENSE_NAMES = YES + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = YES + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = YES + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. If +# EXTRACT_ALL is set to YES then this flag will automatically be disabled. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS +# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but +# at the end of the doxygen process doxygen will return with a non-zero status. +# Possible values are: NO, YES and FAIL_ON_WARNINGS. +# The default value is: NO. + +WARN_AS_ERROR = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: +# https://www.gnu.org/software/libiconv/) for the list of possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# Note the list of default checked file patterns might differ from the list of +# default file extension mappings. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C comment), +# *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, *.vhdl, +# *.ucf, *.qsf and *.ice. + +FILE_PATTERNS = +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = */test/* \ + */.md/* +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = YES + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = ./docs/mainpage.md + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = YES + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# entity all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = YES + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = YES + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see https://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = docs/html + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = doxygen-awesome-css/doxygen-awesome.css \ + doxygen-awesome-css/doxygen-awesome-sidebar-only.css \ + doxygen-awesome-css/doxygen-awesome-sidebar-only-darkmode-toggle.css + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# https://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = YES + +# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML +# documentation will contain a main index with vertical navigation menus that +# are dynamically created via JavaScript. If disabled, the navigation index will +# consists of multiple levels of tabs that are statically embedded in every HTML +# page. Disable this option to support browsers that do not have JavaScript, +# like the Qt help browser. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_MENUS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: +# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To +# create a documentation set, doxygen will generate a Makefile in the HTML +# output directory. Running make will produce the docset in that directory and +# running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy +# genXcode/_index.html for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: +# https://www.microsoft.com/en-us/download/details.aspx?id=21138) on Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the main .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location (absolute path +# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to +# run qhelpgenerator on the generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = YES + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg +# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see +# https://inkscape.org) to generate formulas as SVG images instead of PNGs for +# the HTML output. These images will generally look nicer at scaled resolutions. +# Possible values are: png (the default) and svg (looks nicer but requires the +# pdf2svg or inkscape tool). +# The default value is: png. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FORMULA_FORMAT = png + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands +# to create new LaTeX commands to be used in formulas as building blocks. See +# the section "Including formulas" for details. + +FORMULA_MACROFILE = + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# https://www.mathjax.org) which uses client side JavaScript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from https://www.mathjax.org before deployment. +# The default value is: https://cdn.jsdelivr.net/npm/mathjax@2. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = https://cdn.jsdelivr.net/npm/mathjax@2 + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: +# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /