From c26814673e5a9db3f501a2471c4de380e4a0a743 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 13 Jun 2024 12:39:04 +0200 Subject: [PATCH 01/24] Add nodes Signed-off-by: Simon Eisenmann --- planning/mapless_architecture/README.md | 46 + .../src/autoware_hmi/CMakeLists.txt | 47 + .../src/autoware_hmi/Readme.md | 17 + .../src/autoware_hmi/include/hmi_node.hpp | 55 + .../src/autoware_hmi/launch/hmi.launch.py | 23 + .../src/autoware_hmi/package.xml | 23 + .../src/autoware_hmi/src/hmi_main.cpp | 18 + .../src/autoware_hmi/src/hmi_node.cpp | 84 ++ .../src/autoware_hmi/test/gtest_main.cpp | 9 + .../CMakeLists.txt | 47 + .../src/autoware_local_map_provider/Readme.md | 3 + .../include/local_map_provider_node.hpp | 46 + .../launch/local_map_provider.launch.py | 30 + .../autoware_local_map_provider/package.xml | 25 + .../src/local_map_provider_main.cpp | 18 + .../src/local_map_provider_node.cpp | 39 + .../test/gtest_main.cpp | 9 + .../CMakeLists.txt | 51 + .../autoware_local_mission_planner/Readme.md | 9 + .../include/mission_planner_node.hpp | 345 +++++ .../launch/mission_planner.launch.py | 59 + .../launch/mission_planner_compose.launch.py | 71 ++ .../package.xml | 33 + .../param/mission_planner_default.yaml | 7 + .../src/mission_planner_main.cpp | 16 + .../src/mission_planner_node.cpp | 1126 +++++++++++++++++ .../test/gtest_main.cpp | 54 + .../include/test_mission_planner_core.hpp | 75 ++ .../test/src/test_mission_planner_core.cpp | 653 ++++++++++ .../CMakeLists.txt | 47 + .../autoware_local_road_provider/Readme.md | 3 + .../include/local_road_provider_node.hpp | 47 + .../launch/local_road_provider.launch.py | 27 + .../autoware_local_road_provider/package.xml | 25 + .../src/local_road_provider_main.cpp | 18 + .../src/local_road_provider_node.cpp | 36 + .../test/gtest_main.cpp | 9 + .../CMakeLists.txt | 50 + .../autoware_mission_lane_converter/Readme.md | 6 + .../include/mission_lane_converter_node.hpp | 179 +++ .../launch/mission_lane_converter.launch.py | 67 + .../package.xml | 28 + .../param/mission_lane_converter_default.yaml | 4 + .../src/main_mission_lane_converter.cpp | 18 + .../src/mission_lane_converter_node.cpp | 611 +++++++++ .../test/gtest_main.cpp | 18 + .../test_mission_planner_converter.hpp | 9 + .../src/test_mission_planner_converter.cpp | 113 ++ .../CMakeLists.txt | 78 ++ .../local_mission_planner_common/Readme.md | 3 + .../lib_mission_planner/helper_functions.hpp | 104 ++ .../local_mission_planner_common/package.xml | 25 + .../src/helper_functions.cpp | 179 +++ .../test/gtest_main.cpp | 14 + .../test/include/test_helper_functions.hpp | 10 + .../test/src/test_helper_functions.cpp | 8 + 56 files changed, 4774 insertions(+) create mode 100644 planning/mapless_architecture/README.md create mode 100644 planning/mapless_architecture/src/autoware_hmi/CMakeLists.txt create mode 100644 planning/mapless_architecture/src/autoware_hmi/Readme.md create mode 100644 planning/mapless_architecture/src/autoware_hmi/include/hmi_node.hpp create mode 100644 planning/mapless_architecture/src/autoware_hmi/launch/hmi.launch.py create mode 100644 planning/mapless_architecture/src/autoware_hmi/package.xml create mode 100644 planning/mapless_architecture/src/autoware_hmi/src/hmi_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_hmi/src/hmi_node.cpp create mode 100644 planning/mapless_architecture/src/autoware_hmi/test/gtest_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/CMakeLists.txt create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/Readme.md create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/include/local_map_provider_node.hpp create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/launch/local_map_provider.launch.py create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/package.xml create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_node.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_map_provider/test/gtest_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/CMakeLists.txt create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/Readme.md create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/include/mission_planner_node.hpp create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner.launch.py create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner_compose.launch.py create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/package.xml create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/param/mission_planner_default.yaml create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_node.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/test/gtest_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp create mode 100644 planning/mapless_architecture/src/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/CMakeLists.txt create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/Readme.md create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/include/local_road_provider_node.hpp create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/launch/local_road_provider.launch.py create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/package.xml create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_node.cpp create mode 100644 planning/mapless_architecture/src/autoware_local_road_provider/test/gtest_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/CMakeLists.txt create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/Readme.md create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/package.xml create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/test/gtest_main.cpp create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp create mode 100644 planning/mapless_architecture/src/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/CMakeLists.txt create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/Readme.md create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/package.xml create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/src/helper_functions.cpp create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/test/gtest_main.cpp create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/test/include/test_helper_functions.hpp create mode 100644 planning/mapless_architecture/src/local_mission_planner_common/test/src/test_helper_functions.cpp diff --git a/planning/mapless_architecture/README.md b/planning/mapless_architecture/README.md new file mode 100644 index 0000000000000..7200fa4cd9a7f --- /dev/null +++ b/planning/mapless_architecture/README.md @@ -0,0 +1,46 @@ +[![pipeline status](https://gitlab.com/driveblocks/mod_mission_planner/badges/dev/pipeline.svg)](https://gitlab.com/driveblocks/mod_mission_planner/-/commits/dev) +[![coverage status](https://gitlab.com/driveblocks/mod_mission_planner/badges/dev/coverage.svg)](https://gitlab.com/driveblocks/mod_mission_planner/-/commits/dev) + +# Mission Planner + +The Mission Planner module generates a reference trajectory/path in a local road model based on mission inputs. These inputs are received from the Human Machine Interface (HMI) or another agent. The resulting trajectory or path is then forwarded to the Behavior Planner for further processing. + +A detailed overview can be seen here: + +![mapless_architecture](images/mapless_architecture.svg) + +## Components + +The Mission Planner consists of several components (ROS2 packages) working together: + +- **Mission Planner**: Generates target lanes based on the mission input. +- **HMI (Human Machine Interface)**: Provides a user interface for defining missions via terminal input. +- **Converter**: Converts lanes generated by the Mission Planner into Autoware Trajectories/Paths. + +## Launching the Software + +To launch all nodes of the software: + +``` +ros2 launch mission_planner mission_planner_compose.launch.py +``` + +To launch a specific node, such as the mission planner: + +``` +ros2 launch mission_planner mission_planner.launch.py +``` + +## Additional Notes + +During the beta phase, the mission planner will immediately output a straight trajectory with low velocity to move the vehicle into the local road model. Once the vehicle can be located in the local road model, a trajectory following the ego lane will be computed. + +## Styleguide + +This package adheres to the [Autoware styleguide](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/languages/cpp/) which can be achieved via [`pre-commit`](https://autowarefoundation.github.io/autoware-documentation/pr-347/contributing/pull-request-guidelines/ci-checks/#pre-commit). Run + +``` +pre-commit run -a +``` + +to format the code in the Autoware style and check the tests for errors which have to be solved manually. diff --git a/planning/mapless_architecture/src/autoware_hmi/CMakeLists.txt b/planning/mapless_architecture/src/autoware_hmi/CMakeLists.txt new file mode 100644 index 0000000000000..e4a7957f6f73a --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.8) +project(hmi) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# --- FIND DEPENDENCIES --- +find_package(autoware_cmake REQUIRED) # automatically find dependencies +ament_auto_find_build_dependencies() +autoware_package() + +# build executables +ament_auto_add_executable(${PROJECT_NAME} + src/hmi_main.cpp + src/hmi_node.cpp) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# install executable(s) +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# install launchfile(s) [all within the "launch" folder] +install(DIRECTORY +launch +DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +# configure clang format +set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/mapless_architecture/src/autoware_hmi/Readme.md b/planning/mapless_architecture/src/autoware_hmi/Readme.md new file mode 100644 index 0000000000000..c7d6875070def --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/Readme.md @@ -0,0 +1,17 @@ +# HMI Node + +Creates a mission based on the terminal input (ROS parameter change). + +Available missions: + +- LANE_KEEP +- LANE_CHANGE_LEFT +- LANE_CHANGE_RIGHT +- TAKE_NEXT_EXIT_LEFT +- TAKE_NEXT_EXIT_RIGHT + +Interact with this node by changing the ROS parameters. For a lane change to the right use this command in the terminal: + +``` +ros2 param set /mission_planner/hmi mission LANE_CHANGE_RIGHT +``` diff --git a/planning/mapless_architecture/src/autoware_hmi/include/hmi_node.hpp b/planning/mapless_architecture/src/autoware_hmi/include/hmi_node.hpp new file mode 100644 index 0000000000000..9323a17b0c86a --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/include/hmi_node.hpp @@ -0,0 +1,55 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef HMI_NODE_HPP_ +#define HMI_NODE_HPP_ + +#include "mission_planner_messages/msg/mission.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include + +/** + * Node for HMI. + */ +class HMINode : public rclcpp::Node +{ +public: + /** + * @brief Constructor for the HMINode class. + * Initializes the publisher and subscriber with appropriate topics and QoS + * settings. + */ + HMINode(); + +private: + // ########################################################################### + // # PRIVATE PROCESSING METHODS + // ########################################################################### + + /** + * @brief Callback function for parameter changes. + * This callback function is triggered whenever a ROS 2 parameter is changed. + * @param parameters The received parameter changes. + */ + rcl_interfaces::msg::SetParametersResult ParamCallback_( + const std::vector & parameters); + + /** + * @brief Function which publishes the mission. + * + * @param mission The mission that should be published. + */ + void PublishMission_(std::string mission); + + // ########################################################################### + // # PRIVATE VARIABLES + // ########################################################################### + // Declare ROS2 publisher and subscriber + + rclcpp::Publisher::SharedPtr mission_publisher_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; +}; + +#endif // HMI_NODE_HPP_ diff --git a/planning/mapless_architecture/src/autoware_hmi/launch/hmi.launch.py b/planning/mapless_architecture/src/autoware_hmi/launch/hmi.launch.py new file mode 100644 index 0000000000000..c386c34b954e2 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/launch/hmi.launch.py @@ -0,0 +1,23 @@ +# Copyright 2024 driveblocks GmbH +# driveblocks proprietary license +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + # hmi executable + Node( + package="hmi", + executable="hmi", + name="hmi", + namespace="mission_planner", + remappings=[ + ("hmi_node/output/mission", "hmi_node/output/mission"), + ], + parameters=[], + output="screen", + ), + ] + ) diff --git a/planning/mapless_architecture/src/autoware_hmi/package.xml b/planning/mapless_architecture/src/autoware_hmi/package.xml new file mode 100644 index 0000000000000..4ffb70df35878 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/package.xml @@ -0,0 +1,23 @@ + + + + hmi + 0.0.1 + HMI + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + mission_planner_messages + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mapless_architecture/src/autoware_hmi/src/hmi_main.cpp b/planning/mapless_architecture/src/autoware_hmi/src/hmi_main.cpp new file mode 100644 index 0000000000000..bf2045fdbd9cd --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/src/hmi_main.cpp @@ -0,0 +1,18 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "hmi_node.hpp" + +#include + +#include + +int main(int argc, char * argv[]) +{ + RCLCPP_INFO(rclcpp::get_logger("hmi"), "Launching HMI node..."); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/planning/mapless_architecture/src/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/src/autoware_hmi/src/hmi_node.cpp new file mode 100644 index 0000000000000..f24ba77a56167 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/src/hmi_node.cpp @@ -0,0 +1,84 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "hmi_node.hpp" + +#include "mission_planner_messages/msg/mission.hpp" +#include "rclcpp/rclcpp.hpp" + +using std::placeholders::_1; + +/** + +* @brief Constructor for the HMINode class. + +* Initializes the publisher and subscriber with appropriate topics and QoS +settings. + +*/ +HMINode::HMINode() : Node("hmi_node") +{ + // Set quality of service to best effort (if transmission fails, do not try to + // resend but rather use new sensor data) + // the history_depth is set to 1 (message queue size) + auto qos = rclcpp::QoS(1); + qos.best_effort(); + + // Declare parameter + this->declare_parameter("mission", "LANE_KEEP"); + + // Initialize publisher + mission_publisher_ = + this->create_publisher("hmi_node/output/mission", 1); + + // Initialize parameters callback handle + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&HMINode::ParamCallback_, this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult HMINode::ParamCallback_( + const std::vector & parameters) +{ + // Initialize output + rcl_interfaces::msg::SetParametersResult result; + + result.successful = false; + result.reason = ""; + std::string mission; + for (const auto & param : parameters) { + if (param.get_name() == "mission") { + if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) { + mission = param.as_string(); + + // Publish mission + PublishMission_(mission); + + result.successful = true; + } else { + result.reason = "Incorrect Type"; + } + } + } + return result; +} + +void HMINode::PublishMission_(std::string mission) +{ + mission_planner_messages::msg::Mission missionMessage; + if (mission == "LANE_KEEP") { + missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_KEEP; + } else if (mission == "LANE_CHANGE_LEFT") { + missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_LEFT; + } else if (mission == "LANE_CHANGE_RIGHT") { + missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_RIGHT; + } else if (mission == "TAKE_NEXT_EXIT_LEFT") { + missionMessage.mission_type = mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_LEFT; + } else if (mission == "TAKE_NEXT_EXIT_RIGHT") { + missionMessage.mission_type = mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_RIGHT; + } + + // TODO(simon.eisenmann@driveblocks.ai): Change deadline parameter + missionMessage.deadline = 1000; + + mission_publisher_->publish(missionMessage); +} diff --git a/planning/mapless_architecture/src/autoware_hmi/test/gtest_main.cpp b/planning/mapless_architecture/src/autoware_hmi/test/gtest_main.cpp new file mode 100644 index 0000000000000..d885ea084b1a3 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_hmi/test/gtest_main.cpp @@ -0,0 +1,9 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "gtest/gtest.h" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/CMakeLists.txt b/planning/mapless_architecture/src/autoware_local_map_provider/CMakeLists.txt new file mode 100644 index 0000000000000..8a2de3f45787f --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.8) +project(local_map_provider) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# --- FIND DEPENDENCIES --- +find_package(autoware_cmake REQUIRED) # automatically find dependencies +ament_auto_find_build_dependencies() +autoware_package() + +# build executables +ament_auto_add_executable(${PROJECT_NAME} + src/local_map_provider_main.cpp + src/local_map_provider_node.cpp) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# install executable(s) +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# install launchfile(s) [all within the "launch" folder] +install(DIRECTORY +launch +DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +# configure clang format +set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/Readme.md b/planning/mapless_architecture/src/autoware_local_map_provider/Readme.md new file mode 100644 index 0000000000000..251178ec6cac8 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/Readme.md @@ -0,0 +1,3 @@ +# Local Map Provider Node + +This node converts the mission_planner_messages::msg::RoadSegments message into a mission_planner_messages::msg::LocalMap message right now. More functionality can be added later. diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/include/local_map_provider_node.hpp b/planning/mapless_architecture/src/autoware_local_map_provider/include/local_map_provider_node.hpp new file mode 100644 index 0000000000000..9055d309cfc57 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/include/local_map_provider_node.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef LOCAL_MAP_PROVIDER_NODE_HPP_ +#define LOCAL_MAP_PROVIDER_NODE_HPP_ + +#include "mission_planner_messages/msg/local_map.hpp" +#include "mission_planner_messages/msg/road_segments.hpp" +#include "rclcpp/rclcpp.hpp" + +/** + * Node for the Local Map Provider. + */ +class LocalMapProviderNode : public rclcpp::Node +{ +public: + /** + * @brief Constructor for the LocalMapProviderNode class. + * + * Initializes the publisher and subscriber with appropriate topics and QoS + * settings. + */ + LocalMapProviderNode(); + +private: + // ########################################################################### + // # PRIVATE PROCESSING METHODS + // ########################################################################### + + /** + * @brief The callback for the RoadSegments messages. + * + * @param msg The mission_planner_messages::msg::RoadSegments message. + */ + void CallbackRoadSegmentsMessages_(const mission_planner_messages::msg::RoadSegments & msg); + + // ########################################################################### + // # PRIVATE VARIABLES + // ########################################################################### + // Declare ROS2 publisher and subscriber + + rclcpp::Publisher::SharedPtr map_publisher_; + + rclcpp::Subscription::SharedPtr road_subscriber_; +}; + +#endif // LOCAL_MAP_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/launch/local_map_provider.launch.py b/planning/mapless_architecture/src/autoware_local_map_provider/launch/local_map_provider.launch.py new file mode 100644 index 0000000000000..b26c7fd9b7c81 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/launch/local_map_provider.launch.py @@ -0,0 +1,30 @@ +# Copyright 2024 driveblocks GmbH +# driveblocks proprietary license +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + # local_map_provider executable + Node( + package="local_map_provider", + executable="local_map_provider", + name="local_map_provider", + namespace="mission_planner", + remappings=[ + ( + "local_map_provider_node/output/local_map", + "local_map_provider_node/output/local_map", + ), + ( + "local_map_provider_node/input/road_segments", + "local_road_provider_node/output/road_segments", + ), + ], + parameters=[], + output="screen", + ), + ] + ) diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/package.xml b/planning/mapless_architecture/src/autoware_local_map_provider/package.xml new file mode 100644 index 0000000000000..cf8a6a476bbc8 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/package.xml @@ -0,0 +1,25 @@ + + + + local_map_provider + 0.0.1 + local_map_provider + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + db_msgs + lib_mission_planner + mission_planner_messages + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_main.cpp b/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_main.cpp new file mode 100644 index 0000000000000..99ad196171522 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_main.cpp @@ -0,0 +1,18 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "local_map_provider_node.hpp" + +#include + +#include + +int main(int argc, char * argv[]) +{ + RCLCPP_INFO(rclcpp::get_logger("local_map_provider"), "Launching Local Map Provider node..."); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_node.cpp new file mode 100644 index 0000000000000..b6f4a900ce659 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -0,0 +1,39 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "local_map_provider_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +using std::placeholders::_1; + +LocalMapProviderNode::LocalMapProviderNode() : Node("local_map_provider_node") +{ + // Set quality of service to best effort (if transmission fails, do not try to + // resend but rather use new sensor data) + // the history_depth is set to 1 (message queue size) + auto qos = rclcpp::QoS(1); + qos.best_effort(); + + // Initialize publisher for local map + map_publisher_ = this->create_publisher( + "local_map_provider_node/output/local_map", 1); + + // Initialize subscriber to road segments messages + road_subscriber_ = this->create_subscription( + "local_map_provider_node/input/road_segments", qos, + std::bind(&LocalMapProviderNode::CallbackRoadSegmentsMessages_, this, _1)); +} + +void LocalMapProviderNode::CallbackRoadSegmentsMessages_( + const mission_planner_messages::msg::RoadSegments & msg) +{ + mission_planner_messages::msg::LocalMap local_map; + + // Save road segments in the local map message + local_map.road_segments = msg; + + // Publish the LocalMap message + map_publisher_->publish( + local_map); // Outlook: Add global map, sign detection etc. to the message +} diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/test/gtest_main.cpp b/planning/mapless_architecture/src/autoware_local_map_provider/test/gtest_main.cpp new file mode 100644 index 0000000000000..d885ea084b1a3 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_map_provider/test/gtest_main.cpp @@ -0,0 +1,9 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "gtest/gtest.h" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/CMakeLists.txt b/planning/mapless_architecture/src/autoware_local_mission_planner/CMakeLists.txt new file mode 100644 index 0000000000000..361c943fa79e7 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.8) +project(mission_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# --- FIND DEPENDENCIES --- +find_package(autoware_cmake REQUIRED) # automatically find dependencies +ament_auto_find_build_dependencies() +autoware_package() + +# build executables +ament_auto_add_executable(${PROJECT_NAME} + src/mission_planner_main.cpp + src/mission_planner_node.cpp) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# install executable(s) +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# install launchfile(s)/param file(s) +install(DIRECTORY + launch + param + DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +# configure clang format +set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(mission_planner_messages REQUIRED) + + ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp + test/src/test_mission_planner_core.cpp + src/mission_planner_node.cpp) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/Readme.md b/planning/mapless_architecture/src/autoware_local_mission_planner/Readme.md new file mode 100644 index 0000000000000..e9f2caf9af130 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/Readme.md @@ -0,0 +1,9 @@ +# Mission Planner + +Creates a target lane based on the mission input. The output is forwarded to the converter node. + +Here, one can see the target lane: + +![target lane](images/Targetlane.png) + +The two plots show the point in time, when the lane change was triggered. diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/include/mission_planner_node.hpp b/planning/mapless_architecture/src/autoware_local_mission_planner/include/mission_planner_node.hpp new file mode 100644 index 0000000000000..2eaed7604fdb8 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/include/mission_planner_node.hpp @@ -0,0 +1,345 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef MISSION_PLANNER_NODE_HPP_ +#define MISSION_PLANNER_NODE_HPP_ + +#include "lanelet2_core/geometry/LineString.h" +#include "lanelet_helper/lanelet_converter.hpp" +#include "lanelet_helper/lanelet_geometry.hpp" +#include "lanelet_helper/lanelet_tools.hpp" +#include "lib_mission_planner/helper_functions.hpp" +#include "mission_planner_messages/msg/driving_corridor.hpp" +#include "mission_planner_messages/msg/local_map.hpp" +#include "mission_planner_messages/msg/mission.hpp" +#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" +#include "mission_planner_messages/msg/visualization_distance.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "db_msgs/msg/lanelets_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include + +using namespace lib_mission_planner; + +// Create Direction data type +typedef int Direction; +const Direction stay = 0; +const Direction left = 1; +const Direction right = 2; +const Direction left_most = 3; +const Direction right_most = 4; + +struct Lanes +{ + std::vector ego; + std::vector> left; + std::vector> right; +}; + +/** + * Node for mission planner. + */ +class MissionPlannerNode : public rclcpp::Node +{ +public: + MissionPlannerNode(); + + /** + * @brief Get a point on the given lane that is x meters away in x direction + * (using a projection). + * + * @param lane The given lane (std::vector) on which the point is + * created. + * @param x_distance The point is created x_distance meters (float) away from + * the vehicle (in x direction using a projection). + * @param converted_lanelets The lanelets (std::vector) from + * the road model. + * @return lanelet::BasicPoint2d + */ + lanelet::BasicPoint2d GetPointOnLane_( + const std::vector & lane, const float x_distance, + const std::vector & converted_lanelets); + + /** + * @brief Calculate the distance between a point and a LineString (Euclidean + * distance). + * + * @param linestring The LineString. + * @param point The point. + * @return double + */ + double CalculateDistanceBetweenPointAndLineString_( + const lanelet::ConstLineString2d & linestring, const lanelet::BasicPoint2d & point); + + /** + * @brief Recenter a point in a lanelet to its closest point on the centerline + * + * @param goal_point The input point which should be re-centered + * @param road_model The road model which contains the point to be re-centered + * @return lanelet::BasicPoint2d The re-centered point (which lies on the + * centerline of its lanelet) + */ + lanelet::BasicPoint2d RecenterGoalPoint( + const lanelet::BasicPoint2d & goal_point, const std::vector & road_model); + + /** + * @brief Function which checks if the vehicle is on the goal lane. + * This functions returns a bool depending on whether the vehicle is on the + goal lane or not. + * + * @param ego_lanelet_index The index of the ego lanelet (int). + * @param goal_point The goal point (lanelet::BasicPoint2d). + * @param converted_lanelets The lanelets from the road model + (std::vector). + * @param lanelet_connections The lanelet connections from the road model + (std::vector). + * @return bool (is on goal lane or not). + */ + bool IsOnGoalLane_( + const int ego_lanelet_index, const lanelet::BasicPoint2d & goal_point, + const std::vector & converted_lanelets, + const std::vector & lanelet_connections); + + /** + * @brief Function which checks if the goal point has a negative x value und + * must be therefore reset. If the x value is negative the goal point is reset + * with GetPointOnLane_(). + * + * @param converted_lanelets The lanelets from the road model + (std::vector). + * @param lanelet_connections The lanelet connections from the road model + (std::vector). + */ + void CheckIfGoalPointShouldBeReset_( + const lanelet::Lanelets & converted_lanelets, + const std::vector & lanelet_connections); + + /** + * @brief Function for calculating lanes + * + * @param converted_lanelets The lanelets given from the road model. + * @param lanelet_connections The lanelet connections given from the road + * model. + * @return Lanes: ego lane, all left lanes, all right lanes + */ + Lanes CalculateLanes_( + const std::vector & converted_lanelets, + std::vector & lanelet_connections); + + /** + * @brief Function for creating a marker array. + * This functions creates a visualization_msgs::msg::MarkerArray from the + given input. + * + * @param centerline The centerline which is a LineString. + * @param left The left boundary which is a LineString. + * @param right The right boundary which is a LineString. + * @param msg The LaneletsStamped message. + * @return MarkerArray (visualization_msgs::msg::MarkerArray). + */ + visualization_msgs::msg::MarkerArray CreateMarkerArray_( + const std::vector & centerline, + const std::vector & left, + const std::vector & right, + const mission_planner_messages::msg::RoadSegments & msg); + + /** + * @brief Getter for goal_point_ + * + * @return lanelet::BasicPoint2d + */ + lanelet::BasicPoint2d goal_point(); + + /** + * @brief Setter for goal_point_ + * + * @param goal_point The new value for the goal_point_. + */ + void goal_point(const lanelet::BasicPoint2d & goal_point); + + /** + * @brief Create a DrivingCorridor object. + * + * @param lane The lane which is a std::vector containing all the indices + * of the lane. + * @param converted_lanelets The lanelets (std::vector). + * @return mission_planner_messages::msg::DrivingCorridor + */ + mission_planner_messages::msg::DrivingCorridor CreateDrivingCorridor_( + const std::vector & lane, const std::vector & converted_lanelets); + + /** + * @brief The callback for the Mission messages. + * + * @param msg The mission_planner_messages::msg::Mission message. + */ + void CallbackMissionMessages_(const mission_planner_messages::msg::Mission & msg); + + /** + * @brief The callback for the LocalMap messages. + * + * @param msg The mission_planner_messages::msg::LocalMap message. + */ + void CallbackLocalMapMessages_(const mission_planner_messages::msg::LocalMap & msg); + + /** + * @brief Convert RoadSegments into lanelets. + * + * @param msg The message (mission_planner_messages::msg::RoadSegments). + * @param out_lanelets The lanelets (output). + * @param out_lanelet_connections The lanelet connections (output). + */ + void ConvertInput2LaneletFormat( + const mission_planner_messages::msg::RoadSegments & msg, + std::vector & out_lanelets, + std::vector & out_lanelet_connections); + +private: + // ########################################################################### + // # PRIVATE PROCESSING METHODS + // ########################################################################### + + /** + * @brief Function for the visualization of lanes. + * + * @param msg The db_msgs::msg::LaneletsStamped message. + * @param converted_lanelets The lanelets (std::vector). + */ + void VisualizeLanes_( + const mission_planner_messages::msg::RoadSegments & msg, + const std::vector & converted_lanelets); + + /** + * @brief Function for the visualization of the centerline of a driving corridor. + * + * @param msg The db_msgs::msg::LaneletsStamped message. + * @param driving_corridor The considered driving corridor for which the centerline is visualized. + */ + void VisualizeCenterlineOfDrivingCorridor_( + const mission_planner_messages::msg::RoadSegments & msg, + const mission_planner_messages::msg::DrivingCorridor & driving_corridor); + + /** + * @brief Function for creating a lanelet::LineString2d. + * + * @param points The considered points + * (std::vector). + * @return lanelet::LineString2d + */ + lanelet::LineString2d CreateLineString_(const std::vector & points); + + /** + * @brief Callback for the odometry messages. + * + * @param msg The odometry message (nav_msgs::msg::Odometry). + */ + void CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg); + + /** + * @brief Initiate a lane change. + * + * @param direction The direction of the lane change (-1 for left and +1 for + * right). + * @param neighboring_lane The neighboring lane. + */ + void InitiateLaneChange_(const Direction direction, const std::vector & neighboring_lane); + + /** + * @brief Get all the neighbor lanelets (neighbor lane) of a specific lane on one side. + * + * @param lane The considered lane. + * @param lanelet_connections The lanelet connections. + * @param vehicle_side The side of the vehicle that is considered (enum). + * @return std::vector + */ + std::vector GetAllNeighborsOfLane( + const std::vector & lane, + const std::vector & lanelet_connections, + const int vehicle_side); + + /** + * @brief Add the predecessor lanelet to a lane. + * + * @param lane_idx The considered lane. The predecessor lanelet is added to + * the front of the lane. + * @param lanelet_connections The lanelet connections. + * + */ + void InsertPredecessorLanelet( + std::vector & lane_idx, + const std::vector & lanelet_connections); + + /** + * @brief Calculate the predecessors. + * + * @param lanelet_connections The lanelet connections. + */ + void CalculatePredecessors(std::vector & lanelet_connections); + + // ########################################################################### + // # PRIVATE VARIABLES + // ########################################################################### + // Declare ROS2 publisher and subscriber + rclcpp::Subscription::SharedPtr mapSubscriber_; + + rclcpp::Subscription::SharedPtr missionSubscriber_; + + rclcpp::Publisher::SharedPtr visualizationPublisher_; + + rclcpp::Publisher::SharedPtr + visualization_publisher_centerline_; + + rclcpp::Publisher::SharedPtr + visualizationDistancePublisher_; + + rclcpp::Publisher::SharedPtr visualizationGoalPointPublisher_; + + rclcpp::Publisher::SharedPtr + missionLanesStampedPublisher_; + + rclcpp::Subscription::SharedPtr OdometrySubscriber_; + + // ROS buffer interface (for TF transforms) + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + // store previous odometry message + nav_msgs::msg::Odometry last_odom_msg_; + Pose2D pose_prev_; + // Pose2D target_pose_ = {0.0, 0.0}; + + bool pose_prev_init_ = false; + bool b_global_odometry_deprecation_warning_ = false; + bool received_motion_update_once_ = false; + Direction target_lane_ = stay; + Direction mission_ = stay; + int retry_attempts_ = 0; + bool lane_change_trigger_success_ = true; + Direction lane_change_direction_ = stay; + + lanelet::BasicPoint2d goal_point_; + std::vector ego_lane_; + std::vector lane_left_; + std::vector lane_right_; + std::vector current_lanelets_; + + // ros parameters + float distance_to_centerline_threshold_; + float projection_distance_on_goallane_; + int retrigger_attempts_max_; + + // Unique ID for each marker + int centerline_marker_id_ = 0; +}; + +#endif // MISSION_PLANNER_NODE_HPP_ diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner.launch.py b/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner.launch.py new file mode 100644 index 0000000000000..1d59f5d3ec7a5 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner.launch.py @@ -0,0 +1,59 @@ +# Copyright 2024 driveblocks GmbH +# driveblocks proprietary license +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + mission_planner_param_file = os.path.join( + get_package_share_directory("mission_planner"), "param", "mission_planner_default.yaml" + ) + + mission_planner_param = DeclareLaunchArgument( + "mission_planner_param_file", + default_value=mission_planner_param_file, + description="Path to config file for the mission planner.", + ) + + return LaunchDescription( + [ + # mission planner parameters + mission_planner_param, + # mission_planner executable + Node( + package="mission_planner", + executable="mission_planner", + name="mission_planner", + namespace="mission_planner", + remappings=[ + ( + "mission_planner_node/output/marker", + "mission_planner_node/output/marker", + ), + ( + "mission_planner_node/output/mission_lanes_stamped", + "mission_planner_node/output/mission_lanes_stamped", + ), + ( + "mission_planner_node/input/local_map", + "local_map_provider_node/output/local_map", + ), + ("mission_planner/input/mission", "hmi_node/output/mission"), + ( + "mission_planner/input/state_estimate", + "/awsim/ground_truth/localization/kinematic_state", + ), + ], + parameters=[ + LaunchConfiguration("mission_planner_param_file"), + ], + output="screen", + # prefix="gdbserver localhost:5000", + ), + ] + ) diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner_compose.launch.py b/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner_compose.launch.py new file mode 100644 index 0000000000000..85028833bef37 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner_compose.launch.py @@ -0,0 +1,71 @@ +# Copyright 2024 driveblocks GmbH +# driveblocks proprietary license +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ThisLaunchFileDir + + +def generate_launch_description(): + mission_planner_launch_file = "/mission_planner.launch.py" + + mission_planner = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), mission_planner_launch_file]), + # launch_arguments={'node_name': 'bar'}.items() + ) + + # workaround due to ros2 bug with using ThisLaunchFileDir() + # https://github.com/ros2/launch/issues/618 + # https://gitlab.com/driveblocks/mod_feature_detection/-/merge_requests/102/diffs + mission_lane_converter_pkg_share_directory = get_package_share_directory( + "mission_lane_converter" + ) + mission_lane_converter_file_name = "mission_lane_converter.launch.py" + mission_lane_converter_path = ( + Path(mission_lane_converter_pkg_share_directory) + / "launch/" + / mission_lane_converter_file_name + ) + + mission_lane_converter = IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(mission_lane_converter_path)), + # launch_arguments={'node_name': 'bar'}.items() + ) + + hmi_pkg_share_directory = get_package_share_directory("hmi") + hmi_file_name = "hmi.launch.py" + hmi_path = Path(hmi_pkg_share_directory) / "launch/" / hmi_file_name + + hmi = IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(hmi_path)), + # launch_arguments={'node_name': 'bar'}.items() + ) + + local_road_provider_pkg_share_directory = get_package_share_directory("local_road_provider") + local_road_provider_file_name = "local_road_provider.launch.py" + local_road_provider_path = ( + Path(local_road_provider_pkg_share_directory) / "launch/" / local_road_provider_file_name + ) + + local_road_provider = IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(local_road_provider_path)), + # launch_arguments={'node_name': 'bar'}.items() + ) + + local_map_provider_pkg_share_directory = get_package_share_directory("local_map_provider") + local_map_provider_file_name = "local_map_provider.launch.py" + local_map_provider_path = ( + Path(local_map_provider_pkg_share_directory) / "launch/" / local_map_provider_file_name + ) + + local_map_provider = IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(local_map_provider_path)), + # launch_arguments={'node_name': 'bar'}.items() + ) + + return LaunchDescription( + [mission_planner, mission_lane_converter, hmi, local_road_provider, local_map_provider] + ) diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/src/autoware_local_mission_planner/package.xml new file mode 100644 index 0000000000000..c2314663d155f --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/package.xml @@ -0,0 +1,33 @@ + + + + mission_planner + 0.0.1 + Mission Planner + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + builtin_interfaces + db_msgs + geometry_msgs + lanelet2_core + lanelet_helper + lib_mission_planner + mission_planner_messages + rclcpp + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/param/mission_planner_default.yaml b/planning/mapless_architecture/src/autoware_local_mission_planner/param/mission_planner_default.yaml new file mode 100644 index 0000000000000..b525b14d52bc9 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/param/mission_planner_default.yaml @@ -0,0 +1,7 @@ +/mission_planner: + mission_planner: + ros__parameters: + distance_to_centerline_threshold: 0.25 # [m] threshold to determine if lane change mission was successful + # if ego is in proximity to the goal centerline + projection_distance_on_goallane: 20.0 # [m] projection distance of goal point of a mission + retrigger_attempts_max: 10 # Number of attempts for triggering a lane change diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_main.cpp b/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_main.cpp new file mode 100644 index 0000000000000..5a770e5a29054 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_main.cpp @@ -0,0 +1,16 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "mission_planner_node.hpp" +#include "rclcpp/rclcpp.hpp" + +#include + +int main(int argc, char * argv[]) +{ + RCLCPP_INFO(rclcpp::get_logger("mission_planner"), "Launching mission planner node..."); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_node.cpp new file mode 100644 index 0000000000000..94d117666dd4c --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -0,0 +1,1126 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "mission_planner_node.hpp" + +#include "lanelet2_core/LaneletMap.h" +#include "lanelet2_core/geometry/Lanelet.h" +#include "lanelet2_core/geometry/LineString.h" +#include "lanelet_helper/data_types.hpp" +#include "lanelet_helper/lanelet_converter.hpp" +#include "lanelet_helper/lanelet_geometry.hpp" +#include "lanelet_helper/lanelet_tools.hpp" +#include "lib_mission_planner/helper_functions.hpp" +#include "mission_planner_messages/msg/driving_corridor.hpp" +#include "mission_planner_messages/msg/mission.hpp" +#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "db_msgs/msg/lanelets_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using std::placeholders::_1; +using namespace lib_mission_planner; + +MissionPlannerNode::MissionPlannerNode() : Node("mission_planner_node") +{ + // Set quality of service to best effort (if transmission fails, do not try to + // resend but rather use new sensor data) + // the history_depth is set to 1 (message queue size) + auto qos = rclcpp::QoS(1); + qos.best_effort(); + + // Initialize publisher for visualization markers + visualizationPublisher_ = this->create_publisher( + "mission_planner_node/output/marker", 1); + + // Initialize publisher to visualize the centerline of a lane + visualization_publisher_centerline_ = + this->create_publisher( + "mission_planner_node/output/centerline", 1); + + // Initialize publisher for visualization of the distance + visualizationDistancePublisher_ = + this->create_publisher( + "mission_planner_node/output/visualization_distance", 1); + + // Initialize publisher for goal point marker + visualizationGoalPointPublisher_ = this->create_publisher( + "mission_planner_node/output/marker_goal_point", 1); + + // Initialize publisher for mission lanes + missionLanesStampedPublisher_ = + this->create_publisher( + "mission_planner_node/output/mission_lanes_stamped", 1); + + // Initialize subscriber to lanelets stamped messages + mapSubscriber_ = this->create_subscription( + "mission_planner_node/input/local_map", qos, + std::bind(&MissionPlannerNode::CallbackLocalMapMessages_, this, _1)); + + // Initialize subscriber to mission messages + missionSubscriber_ = this->create_subscription( + "mission_planner/input/mission", qos, + std::bind(&MissionPlannerNode::CallbackMissionMessages_, this, _1)); + + // Initialize subscriber to odometry messages + OdometrySubscriber_ = this->create_subscription( + "mission_planner/input/state_estimate", qos, + std::bind(&MissionPlannerNode::CallbackOdometryMessages_, this, _1)); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + + // Set ros parameters (DEFAULT values will be overwritten by external + // parameter file if exists) + distance_to_centerline_threshold_ = + declare_parameter("distance_to_centerline_threshold", 0.2); + RCLCPP_INFO( + this->get_logger(), "Threshold distance to centerline for successful lane change: %.2f", + distance_to_centerline_threshold_); + + projection_distance_on_goallane_ = + declare_parameter("projection_distance_on_goallane", 30.0); + RCLCPP_INFO( + this->get_logger(), "Projection distance for goal point in mission: %.1f", + projection_distance_on_goallane_); + + retrigger_attempts_max_ = declare_parameter("retrigger_attempts_max", 10); + RCLCPP_INFO( + this->get_logger(), "Number of attempts for triggering a lane change: %d", + retrigger_attempts_max_); +} + +void MissionPlannerNode::CallbackLocalMapMessages_( + const mission_planner_messages::msg::LocalMap & msg) +{ + // Used for output + std::vector lanelet_connections; + std::vector converted_lanelets; + + ConvertInput2LaneletFormat(msg.road_segments, converted_lanelets, lanelet_connections); + MissionPlannerNode::VisualizeLanes_(msg.road_segments, converted_lanelets); + + // Get the lanes + Lanes result = MissionPlannerNode::CalculateLanes_(converted_lanelets, lanelet_connections); + + // Get the ego lane + ego_lane_ = result.ego; + + std::vector> left_lanes = result.left; + if (!left_lanes.empty()) { + lane_left_ = left_lanes[0]; // Store the first left lane (needed for lane change) + } + + std::vector> right_lanes = result.right; + if (!right_lanes.empty()) { + lane_right_ = right_lanes[0]; // Store the first right lane (needed for lane change) + } + + // Save current converted_lanelets + current_lanelets_ = converted_lanelets; + + // Re-trigger lane change if necessary + if (lane_change_trigger_success_ == false && retry_attempts_ <= retrigger_attempts_max_) { + if (lane_change_direction_ == left) { + // Lane change to the left + InitiateLaneChange_(left, lane_left_); + } else if (lane_change_direction_ == right) { + // Lane change to the right + InitiateLaneChange_(right, lane_right_); + } + } + + if (retry_attempts_ > retrigger_attempts_max_) { + // Lane change has failed, must be re-triggered manually + RCLCPP_WARN( + this->get_logger(), "Lane change failed! Number of attempts: (%d/%d)", retry_attempts_, + retrigger_attempts_max_); + + // Reset variable + lane_change_trigger_success_ = true; + retry_attempts_ = 0; + } + + // Check if goal point should be reset, if yes -> reset goal point + CheckIfGoalPointShouldBeReset_(converted_lanelets, lanelet_connections); + + // Check if lane change was successful, if yes -> reset mission + if (mission_ != stay) { + int egoLaneletIndex = + path_geometry::FindEgoOccupiedLaneletID(converted_lanelets); // Returns -1 if no match + lanelet::BasicPoint2d pointEgo(0, + 0); // Vehicle is always located at (0, 0) + + if (egoLaneletIndex >= 0) { + // Check if successful lane change + if ( + IsOnGoalLane_(egoLaneletIndex, goal_point_, converted_lanelets, lanelet_connections) && + CalculateDistanceBetweenPointAndLineString_( + converted_lanelets[egoLaneletIndex].centerline2d(), pointEgo) <= + distance_to_centerline_threshold_) { + // Reset mission to lane keeping + mission_ = stay; + target_lane_ = stay; + } + + // Check if we are on the target lane, if yes -> update target_lane + if (IsOnGoalLane_(egoLaneletIndex, goal_point_, converted_lanelets, lanelet_connections)) { + target_lane_ = stay; + } else { + target_lane_ = mission_; + } + } + } + + // Create a MarkerArray for clearing old markers + visualization_msgs::msg::Marker clear_marker; + clear_marker.action = visualization_msgs::msg::Marker::DELETEALL; + + visualization_msgs::msg::MarkerArray clear_marker_array; + clear_marker_array.markers.push_back(clear_marker); + + // Publish the clear marker array to delete old markers + visualization_publisher_centerline_->publish(clear_marker_array); + + // Publish mission lanes + mission_planner_messages::msg::MissionLanesStamped lanes; + lanes.header.frame_id = msg.road_segments.header.frame_id; // Same frame_id as msg + lanes.header.stamp = rclcpp::Node::now(); + + // Add target lane + switch (target_lane_) { + case stay: + lanes.target_lane = 0; + break; + case left: + lanes.target_lane = -1; + break; + case right: + lanes.target_lane = +1; + break; + case left_most: + lanes.target_lane = -2; + break; + case right_most: + lanes.target_lane = +2; + break; + + default: + break; + } + + lanes.deadline_target_lane = + std::numeric_limits::infinity(); // TODO(simon.eisenmann@driveblocks.ai): + // Change this value + + // Create driving corridors and add them to the MissionLanesStamped message + lanes.ego_lane = MissionPlannerNode::CreateDrivingCorridor_(ego_lane_, converted_lanelets); + MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_(msg.road_segments, lanes.ego_lane); + + // Initialize driving corridor + mission_planner_messages::msg::DrivingCorridor driving_corridor; + + if (!left_lanes.empty()) { + for (const std::vector & lane : left_lanes) { + driving_corridor = MissionPlannerNode::CreateDrivingCorridor_(lane, converted_lanelets); + lanes.drivable_lanes_left.push_back(driving_corridor); + MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( + msg.road_segments, driving_corridor); + } + } + + if (!right_lanes.empty()) { + for (const std::vector & lane : right_lanes) { + driving_corridor = MissionPlannerNode::CreateDrivingCorridor_(lane, converted_lanelets); + lanes.drivable_lanes_right.push_back(driving_corridor); + MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( + msg.road_segments, driving_corridor); + } + } + + // Publish MissionLanesStamped message + missionLanesStampedPublisher_->publish(lanes); +} + +void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg) +{ + // NOTE: We assume that the odometry message is the GNSS signal + + // Construct raw odometry pose + geometry_msgs::msg::PoseStamped odometry_pose_raw, pose_base_link_in_odom_frame, + pose_base_link_in_map_frame; + odometry_pose_raw.header = msg.header; + odometry_pose_raw.pose = msg.pose.pose; + + // If the incoming odometry signal is properly filled, i.e. if the frame ids + // are given and report an odometry signal , do nothing, else we assume the + // odometry signal stems from the GNSS (and is therefore valid in the odom + // frame) + if (msg.header.frame_id == "map" && msg.child_frame_id == "base_link") { + pose_base_link_in_map_frame = odometry_pose_raw; + } else { + if (!b_global_odometry_deprecation_warning_) { + RCLCPP_WARN( + this->get_logger(), + "Your odometry signal doesn't match the expectation to be a " + "transformation from frame to ! We assume the " + "input " + "signal to be a GNSS raw signal being a transform from to " + ". The support for this feature will be deprecated in a " + "future " + "release, please check you odometry signal or use a driveblocks " + "local odometry signal instead! This warning is printed only " + "once."); + b_global_odometry_deprecation_warning_ = true; + } + // Prepare map to odom transform + // TODO(thomas.herrmann@driveblocks.ai): Can be removed when the state + // estimator publishes this information in the correct frames + geometry_msgs::msg::TransformStamped trafo_map2odom; + trafo_map2odom.header.stamp = msg.header.stamp; + trafo_map2odom.header.frame_id = "map"; + trafo_map2odom.child_frame_id = "odom"; + trafo_map2odom.transform.translation.x = msg.pose.pose.position.x; + trafo_map2odom.transform.translation.y = msg.pose.pose.position.y; + trafo_map2odom.transform.translation.z = msg.pose.pose.position.z; + trafo_map2odom.transform.rotation.x = msg.pose.pose.orientation.x; + trafo_map2odom.transform.rotation.y = msg.pose.pose.orientation.y; + trafo_map2odom.transform.rotation.z = msg.pose.pose.orientation.z; + trafo_map2odom.transform.rotation.w = msg.pose.pose.orientation.w; + + geometry_msgs::msg::TransformStamped trafo_base_link_in_odom_frame; + try { + // constant trafo from gnss receiver to base_link + trafo_base_link_in_odom_frame = + tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(this->get_logger(), "Transform not yet available from to "); + } + + // Extract the trafo from the odom frame to the base_link frame + pose_base_link_in_odom_frame.header.frame_id = "odom"; + pose_base_link_in_odom_frame.pose.position.x = + trafo_base_link_in_odom_frame.transform.translation.x; + pose_base_link_in_odom_frame.pose.position.y = + trafo_base_link_in_odom_frame.transform.translation.y; + pose_base_link_in_odom_frame.pose.position.z = + trafo_base_link_in_odom_frame.transform.translation.z; + pose_base_link_in_odom_frame.pose.orientation.x = + trafo_base_link_in_odom_frame.transform.rotation.x; + pose_base_link_in_odom_frame.pose.orientation.y = + trafo_base_link_in_odom_frame.transform.rotation.y; + pose_base_link_in_odom_frame.pose.orientation.z = + trafo_base_link_in_odom_frame.transform.rotation.z; + pose_base_link_in_odom_frame.pose.orientation.w = + trafo_base_link_in_odom_frame.transform.rotation.w; + + // Transform base_link origin from odom frame to map frame + tf2::doTransform(pose_base_link_in_odom_frame, pose_base_link_in_map_frame, trafo_map2odom); + } + + // Calculate yaw for received pose + double psi_cur_corrected = GetYawFromQuaternion( + pose_base_link_in_map_frame.pose.orientation.x, pose_base_link_in_map_frame.pose.orientation.y, + pose_base_link_in_map_frame.pose.orientation.z, pose_base_link_in_map_frame.pose.orientation.w); + + RCLCPP_DEBUG( + this->get_logger(), + "GNSS pose raw: x,y,z | quaternion: %.3f, %.3f, %.3f | %.3f, %.3f, " + "%.3f, " + "%.3f\t " + "GNSS pose in base_link: x,y,z | quaternion: %.3f, %.3f, %.3f | %.3f, " + "%.3f, " + "%.3f, %.3f", + odometry_pose_raw.pose.position.x, odometry_pose_raw.pose.position.y, + odometry_pose_raw.pose.position.z, odometry_pose_raw.pose.orientation.x, + odometry_pose_raw.pose.orientation.y, odometry_pose_raw.pose.orientation.z, + odometry_pose_raw.pose.orientation.w, pose_base_link_in_map_frame.pose.position.x, + pose_base_link_in_map_frame.pose.position.y, pose_base_link_in_map_frame.pose.position.z, + pose_base_link_in_map_frame.pose.orientation.x, pose_base_link_in_map_frame.pose.orientation.y, + pose_base_link_in_map_frame.pose.orientation.z, pose_base_link_in_map_frame.pose.orientation.w); + + RCLCPP_DEBUG( + this->get_logger(), "Received pose (x: %.2f, y: %.2f, psi %.2f)", + pose_base_link_in_map_frame.pose.position.x, pose_base_link_in_map_frame.pose.position.y, + psi_cur_corrected); + + if (pose_prev_init_) { + // Calculate and forward relative motion update to driving corridor model + const Pose2D pose_cur( + pose_base_link_in_map_frame.pose.position.x, pose_base_link_in_map_frame.pose.position.y, + psi_cur_corrected); + const Pose2D d_pose = TransformToNewCosy2D(pose_prev_, pose_cur); + + // Transform the target pose into the new cosy which is given in relation + // to the previous origin + Pose2D target_pose = {goal_point_.x(), goal_point_.y()}; + target_pose = TransformToNewCosy2D(d_pose, target_pose); + + // Overwrite goal point + goal_point_.x() = target_pose.get_x(); + goal_point_.y() = target_pose.get_y(); + + // Re-center updated goal point to lie on centerline (to get rid of issues + // with a less accurate odometry update which could lead to loosing the + // goal lane) + // TODO(thomas.herrmann@driveblocks.ai): Reduction of calling + // frequency of this method (since not needed at a high frequency and + // probably computationally expensive) + const lanelet::BasicPoint2d target_point_2d = RecenterGoalPoint(goal_point_, current_lanelets_); + + // Overwrite goal point + goal_point_.x() = target_point_2d.x(); + goal_point_.y() = target_point_2d.y(); + + // --- Start of debug visualization + // Create marker and publish it + visualization_msgs::msg::Marker goal_marker; // Create a new marker + + goal_marker.header.frame_id = msg.header.frame_id; + goal_marker.header.stamp = msg.header.stamp; + goal_marker.ns = "goal_point"; + goal_marker.type = visualization_msgs::msg::Marker::POINTS; + goal_marker.action = visualization_msgs::msg::Marker::ADD; + goal_marker.pose.orientation.w = 1.0; // Neutral orientation + goal_marker.scale.x = 6.0; + goal_marker.color.r = 1.0; // Red color + goal_marker.color.a = 1.0; // Full opacity + + // Add goal point to the marker + geometry_msgs::msg::Point p_marker; + + p_marker.x = goal_point_.x(); + p_marker.y = goal_point_.y(); + + goal_marker.points.push_back(p_marker); + + // Clear all markers in scene + visualization_msgs::msg::Marker msg_marker; + msg_marker.header = msg.header; + msg_marker.type = visualization_msgs::msg::Marker::POINTS; + // This specifies the clear all / delete all action + msg_marker.action = 3; + visualizationGoalPointPublisher_->publish(msg_marker); + + visualizationGoalPointPublisher_->publish(goal_marker); + // --- End of debug visualization + + last_odom_msg_ = msg; + } else { + pose_prev_init_ = true; + last_odom_msg_ = msg; + } + + // Update pose storage for next iteration + pose_prev_.set_x(pose_base_link_in_map_frame.pose.position.x); + pose_prev_.set_y(pose_base_link_in_map_frame.pose.position.y); + pose_prev_.set_psi(psi_cur_corrected); + + received_motion_update_once_ = true; + + return; +} + +lanelet::BasicPoint2d MissionPlannerNode::RecenterGoalPoint( + const lanelet::BasicPoint2d & goal_point, const std::vector & road_model) +{ + // Return value + lanelet::BasicPoint2d projected_goal_point; + + // Get current lanelet index of goal point + const int lanelet_idx_goal_point = path_geometry::FindOccupiedLaneletID(road_model, goal_point); + + if (lanelet_idx_goal_point >= 0) { + // Get the centerline of the goal point's lanelet + lanelet::ConstLineString2d centerline_curr_lanelet = + road_model[lanelet_idx_goal_point].centerline2d(); + + // Project goal point to the centerline of its lanelet + projected_goal_point = lanelet::geometry::project(centerline_curr_lanelet, goal_point); + } else { + // Return untouched input point if index is not valid + projected_goal_point = goal_point; + } + + return projected_goal_point; +} + +void MissionPlannerNode::CallbackMissionMessages_( + const mission_planner_messages::msg::Mission & msg) +{ + // Initialize variables + lane_change_trigger_success_ = false; + retry_attempts_ = 0; + + switch (msg.mission_type) { + case mission_planner_messages::msg::Mission::LANE_KEEP: + // Keep the lane + mission_ = stay; + target_lane_ = stay; + break; + case mission_planner_messages::msg::Mission::LANE_CHANGE_LEFT: + // Initiate left lane change + RCLCPP_INFO(this->get_logger(), "Lane change to the left initiated."); + lane_change_direction_ = left; + InitiateLaneChange_(lane_change_direction_, lane_left_); + break; + case mission_planner_messages::msg::Mission::LANE_CHANGE_RIGHT: + // Initiate right lane change + RCLCPP_INFO(this->get_logger(), "Lane change to the right initiated."); + lane_change_direction_ = right; + InitiateLaneChange_(lane_change_direction_, lane_right_); + break; + case mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_LEFT: + // Initiate take next exit + RCLCPP_INFO(this->get_logger(), "Take next exit (left) initiated."); + target_lane_ = left_most; // Set target lane + break; + case mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_RIGHT: + // Initiate take next exit + RCLCPP_INFO(this->get_logger(), "Take next exit (right) initiated."); + target_lane_ = right_most; // Set target lane + break; + default: + // Nothing happens if mission does not match! + RCLCPP_INFO(this->get_logger(), "Mission does not match."); + } + + return; +} + +void MissionPlannerNode::InitiateLaneChange_( + const Direction direction, const std::vector & neighboring_lane) +{ + retry_attempts_++; // Increment retry attempts counter + if (neighboring_lane.size() == 0) { + // Neighbor lane is empty + RCLCPP_WARN(this->get_logger(), "Empty neighbor lane!"); + } else { + // Neighbor lane is available, initiate lane change + RCLCPP_WARN(this->get_logger(), "Lane change successfully triggered!"); + lane_change_trigger_success_ = true; + mission_ = direction; + target_lane_ = direction; + goal_point_ = + GetPointOnLane_(neighboring_lane, projection_distance_on_goallane_, current_lanelets_); + } +} + +void MissionPlannerNode::VisualizeLanes_( + const mission_planner_messages::msg::RoadSegments & msg, + const std::vector & converted_lanelets) +{ + // Calculate centerlines, left and right bounds + std::vector centerlines; + std::vector left_bounds; + std::vector right_bounds; + + // Go through every lanelet + for (const lanelet::Lanelet & l : converted_lanelets) { + auto centerline = l.centerline(); + auto bound_left = l.leftBound(); + auto bound_right = l.rightBound(); + + centerlines.push_back(centerline); + left_bounds.push_back(bound_left); + right_bounds.push_back(bound_right); + } + + auto marker_array = + MissionPlannerNode::CreateMarkerArray_(centerlines, left_bounds, right_bounds, msg); + + // Publish centerlines, left and right bounds + visualizationPublisher_->publish(marker_array); +} + +void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( + const mission_planner_messages::msg::RoadSegments & msg, + const mission_planner_messages::msg::DrivingCorridor & driving_corridor) +{ + // Create a marker for the centerline + visualization_msgs::msg::Marker centerline_marker; + centerline_marker.header.frame_id = msg.header.frame_id; + centerline_marker.header.stamp = msg.header.stamp; + centerline_marker.ns = "centerline"; + + // Unique ID + if (centerline_marker_id_ == std::numeric_limits::max()) { + // Handle overflow + centerline_marker_id_ = 0; + } else { + centerline_marker.id = centerline_marker_id_++; + } + + centerline_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + centerline_marker.action = visualization_msgs::msg::Marker::ADD; + + // Set the scale of the marker + centerline_marker.scale.x = 0.1; // Line width + + // Set the color of the marker (red, green, blue, alpha) + centerline_marker.color.r = 1.0; + centerline_marker.color.g = 0.0; + centerline_marker.color.b = 0.0; + centerline_marker.color.a = 1.0; + + // Add points to the marker + for (const geometry_msgs::msg::Point & p : driving_corridor.centerline) { + centerline_marker.points.push_back(p); + } + + // Create a MarkerArray to hold the marker + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.push_back(centerline_marker); + + // Publish the marker array + visualization_publisher_centerline_->publish(marker_array); +} + +// Determine the lanes +Lanes MissionPlannerNode::CalculateLanes_( + const std::vector & converted_lanelets, + std::vector & lanelet_connections) +{ + // Calculate centerlines, left and right bounds + std::vector centerlines; + std::vector left_bounds; + std::vector right_bounds; + + int ego_lanelet_index = path_geometry::FindEgoOccupiedLaneletID( + converted_lanelets); // Finds the ID of the ego vehicle occupied + // lanelet (returns -1 if no match) + + // Initialize variables + std::vector> ego_lane; + std::vector ego_lane_stripped_idx = {}; + std::vector> left_lanes = {}; + std::vector> right_lanes = {}; + + if (ego_lanelet_index >= 0) { + // Get ego lane + ego_lane = lanelet_tools::GetAllSuccessorSequences(lanelet_connections, ego_lanelet_index); + + // Extract the first available ego lane + if (ego_lane.size() > 0) { + ego_lane_stripped_idx = ego_lane[0]; + + // Get all neighbor lanelets to the ego lanelet on the left side + std::vector left_neighbors = lanelet_tools::GetAllNeighboringLaneletIDs( + lanelet_connections, ego_lanelet_index, lanelet_types::VehicleSide::kLeft); + + // Initialize current_lane and next_lane + std::vector current_lane = ego_lane_stripped_idx; + std::vector neighbor_lane; + + for (size_t i = 0; i < left_neighbors.size(); ++i) { + neighbor_lane = GetAllNeighborsOfLane( + current_lane, lanelet_connections, lanelet_types::VehicleSide::kLeft); + + left_lanes.push_back(neighbor_lane); + + current_lane = neighbor_lane; + } + + // Get all neighbor lanelets to the ego lanelet on the right side + std::vector right_neighbors = lanelet_tools::GetAllNeighboringLaneletIDs( + lanelet_connections, ego_lanelet_index, lanelet_types::VehicleSide::kRight); + + // Reinitialize current_lane + current_lane = ego_lane_stripped_idx; + + for (size_t i = 0; i < right_neighbors.size(); ++i) { + neighbor_lane = GetAllNeighborsOfLane( + current_lane, lanelet_connections, lanelet_types::VehicleSide::kRight); + + right_lanes.push_back(neighbor_lane); + + current_lane = neighbor_lane; + } + } + } + + // Add one predecessor lanelet to the ego lane + InsertPredecessorLanelet(ego_lane_stripped_idx, lanelet_connections); + + // Add one predecessor lanelet to each of the left lanes + for (std::vector & lane : left_lanes) { + InsertPredecessorLanelet(lane, lanelet_connections); + } + + // Add one predecessor lanelet to each of the right lanes + for (std::vector & lane : right_lanes) { + InsertPredecessorLanelet(lane, lanelet_connections); + } + + // Return lanes + Lanes lanes; + lanes.ego = ego_lane_stripped_idx; + lanes.left = left_lanes; + lanes.right = right_lanes; + + return lanes; +} + +void MissionPlannerNode::InsertPredecessorLanelet( + std::vector & lane_idx, + const std::vector & lanelet_connections) +{ + if (!lane_idx.empty()) { + // Get index of first lanelet + int first_lanelet_index = lane_idx[0]; + + if (!lanelet_connections[first_lanelet_index].predecessor_lanelet_ids.empty()) { + // Get one predecessor lanelet + const int predecessor_lanelet = lanelet_connections[first_lanelet_index] + .predecessor_lanelet_ids[0]; // Get one of the predecessors + + // Insert predecessor lanelet in lane_idx + if (predecessor_lanelet >= 0) { + lane_idx.insert(lane_idx.begin(), predecessor_lanelet); + } + } + } +} + +std::vector MissionPlannerNode::GetAllNeighborsOfLane( + const std::vector & lane, + const std::vector & lanelet_connections, const int vehicle_side) +{ + // Initialize vector + std::vector neighbor_lane_idx = {}; + + if (!lane.empty()) { + // Loop through all the lane indices to get the neighbors + int neighbor_tmp; + + for (const int id : lane) { + neighbor_tmp = lanelet_connections[id].neighbor_lanelet_ids[vehicle_side]; + if (neighbor_tmp >= 0) { + // Only add neighbor if lanelet does not exist already (avoid having + // duplicates) + if ( + std::find(neighbor_lane_idx.begin(), neighbor_lane_idx.end(), neighbor_tmp) == + neighbor_lane_idx.end()) { + neighbor_lane_idx.push_back(neighbor_tmp); + } + } else { + // If there is a blind spot in the neighbor sequence, break the loop + break; + } + } + } + + return neighbor_lane_idx; +} + +visualization_msgs::msg::MarkerArray MissionPlannerNode::CreateMarkerArray_( + const std::vector & centerline, + const std::vector & left, + const std::vector & right, + const mission_planner_messages::msg::RoadSegments & msg) +{ + visualization_msgs::msg::MarkerArray markerArray; + + // Centerline + for (size_t i = 0; i < centerline.size(); ++i) { + visualization_msgs::msg::Marker marker; // Create a new marker in each iteration + + // Adding points to the marker + for (const auto & point : centerline[i]) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + marker.points.push_back(p); + } + markerArray.markers.push_back(marker); + } + + // Left bound + for (size_t i = 0; i < left.size(); ++i) { + visualization_msgs::msg::Marker marker; // Create a new marker in each iteration + + marker.header.frame_id = msg.header.frame_id; // Adjust frame_id as needed + marker.header.stamp = msg.header.stamp; // rclcpp::Node::now(); + marker.ns = "linestring"; + marker.id = i + 1000; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; // Neutral orientation + marker.scale.x = 5.0; + marker.color.b = 1.0; // Blue color + marker.color.a = 1.0; // Full opacity + + // Adding points to the marker + for (const auto & point : left[i]) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + marker.points.push_back(p); + } + markerArray.markers.push_back(marker); + } + + // Right bound + for (size_t i = 0; i < right.size(); ++i) { + visualization_msgs::msg::Marker marker; // Create a new marker in each iteration + + marker.header.frame_id = msg.header.frame_id; // Adjust frame_id as needed + marker.header.stamp = msg.header.stamp; // rclcpp::Node::now(); + marker.ns = "linestring"; + marker.id = i + 2000; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; // Neutral orientation + marker.scale.x = 5.0; + marker.color.b = 1.0; // Blue color + marker.color.a = 1.0; // Full opacity + + // Adding points to the marker + for (const auto & point : right[i]) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + marker.points.push_back(p); + } + markerArray.markers.push_back(marker); + } + + return markerArray; +} + +mission_planner_messages::msg::DrivingCorridor MissionPlannerNode::CreateDrivingCorridor_( + const std::vector & lane, const std::vector & converted_lanelets) +{ + // Create driving corridor + mission_planner_messages::msg::DrivingCorridor driving_corridor; + + for (int id : lane) { + if (id >= 0) { + auto current_lanelet = converted_lanelets.at(id); + + auto centerline = current_lanelet.centerline(); + auto bound_left = current_lanelet.leftBound(); + auto bound_right = current_lanelet.rightBound(); + + // Adding elements of centerline + for (const auto & point : centerline) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + driving_corridor.centerline.push_back(p); + } + + // Adding elements of bound_left + for (const auto & point : bound_left) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + driving_corridor.bound_left.push_back(p); + } + + // Adding elements of bound_right + for (const auto & point : bound_right) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + driving_corridor.bound_right.push_back(p); + } + } + } + return driving_corridor; +} + +lanelet::BasicPoint2d MissionPlannerNode::GetPointOnLane_( + const std::vector & lane, const float x_distance, + const std::vector & converted_lanelets) +{ + lanelet::BasicPoint2d return_point; // return value + + if (lane.size() > 0) { + lanelet::ConstLineString2d linestring = MissionPlannerNode::CreateLineString_( + MissionPlannerNode::CreateDrivingCorridor_(lane, converted_lanelets) + .centerline); // Create linestring for the lane + + // Create point that is float meters in front (x axis) + lanelet::BasicPoint2d point(x_distance, 0.0); + + // Get projected point on the linestring + lanelet::BasicPoint2d projected_point = lanelet::geometry::project(linestring, point); + + // Overwrite p (return value) + return_point.x() = projected_point.x(); + return_point.y() = projected_point.y(); + } else { + RCLCPP_WARN( + this->get_logger(), + "Overwriting of point may not have occurred properly. Lane is " + "probably empty."); + } + + // Return point p + return return_point; +} + +bool MissionPlannerNode::IsOnGoalLane_( + const int ego_lanelet_index, const lanelet::BasicPoint2d & goal_point, + const std::vector & converted_lanelets, + const std::vector & lanelet_connections) +{ + bool result = false; + + // Find the index of the lanelet containing the goal point + int goal_index = + path_geometry::FindOccupiedLaneletID(converted_lanelets, goal_point); // Returns -1 if no match + + if (goal_index >= 0) { // Check if -1 + std::vector> goal_lane = lanelet_tools::GetAllPredecessorSequences( + lanelet_connections, + goal_index); // Get goal lane + + // Check if vehicle is on goal lane + if (ego_lanelet_index == goal_index) { + result = true; + } + + for (const auto & innerVec : goal_lane) { + for (int value : innerVec) { + if (value == ego_lanelet_index) { + result = true; + } + } + } + } + return result; +} + +// Create LineString2d that consists of the given points (x and y) +lanelet::LineString2d MissionPlannerNode::CreateLineString_( + const std::vector & points) +{ + // Create a Lanelet2 linestring + lanelet::LineString2d linestring; + + // Iterate through the vector of points and add them to the linestring + for (const auto & point : points) { + lanelet::Point2d p(0, point.x, + point.y); // First argument is ID (set it to 0 for now) + linestring.push_back(p); + } + + // Return the created linestring + return linestring; +} + +// Function to calculate the shortest distance between a point and a +// linestring +double MissionPlannerNode::CalculateDistanceBetweenPointAndLineString_( + const lanelet::ConstLineString2d & linestring, const lanelet::BasicPoint2d & point) +{ + // Get projected point on the linestring + lanelet::BasicPoint2d projected_point = lanelet::geometry::project(linestring, point); + + // Calculate the distance between the two points + double distance = lanelet::geometry::distance2d(point, projected_point); + + // Publish distance + mission_planner_messages::msg::VisualizationDistance d; + d.distance = distance; + visualizationDistancePublisher_->publish(d); + + return distance; +} + +void MissionPlannerNode::CheckIfGoalPointShouldBeReset_( + const lanelet::Lanelets & converted_lanelets, + const std::vector & lanelet_connections) +{ + // Check if goal point should be reset: If the x value of the goal point is + // negative, then the point is behind the vehicle and must be therefore reset. + if (goal_point_.x() < 0 && mission_ != stay) { // TODO(simon.eisenmann@driveblocks.ai): Maybe + // remove condition mission_ != stay + // Find the index of the lanelet containing the goal point + int goal_index = path_geometry::FindOccupiedLaneletID( + converted_lanelets, goal_point_); // Returns -1 if no match + + if (goal_index >= 0) { // Check if -1 + // Reset goal point + goal_point_ = GetPointOnLane_( + lanelet_tools::GetAllSuccessorSequences(lanelet_connections, goal_index)[0], + projection_distance_on_goallane_, converted_lanelets); + } else { + // Reset of goal point not successful -> reset mission and target lane + RCLCPP_WARN(this->get_logger(), "Lanelet of goal point cannot be determined, mission reset!"); + + target_lane_ = 0; + mission_ = 0; + } + } +} + +// Getter for goal_point_ +lanelet::BasicPoint2d MissionPlannerNode::goal_point() +{ + return goal_point_; +} + +// Setter for goal_point_ +void MissionPlannerNode::goal_point(const lanelet::BasicPoint2d & goal_point) +{ + goal_point_ = goal_point; +} + +void MissionPlannerNode::ConvertInput2LaneletFormat( + const mission_planner_messages::msg::RoadSegments & msg, + std::vector & out_lanelets, + std::vector & out_lanelet_connections) +{ + // Local variables + const unsigned int n_linestrings_per_lanelet = 2; + // Left/right boundary of a lanelet + std::vector la_linestrings(n_linestrings_per_lanelet); + // Points per linestring + std::vector ls_points = {}; + + // Store mapping from original lanelet ids to new (index-based) ids + std::unordered_map map_original_to_new; + + out_lanelets.reserve(msg.segments.size()); + out_lanelet_connections.reserve(msg.segments.size()); + + for (size_t idx_segment = 0; idx_segment < msg.segments.size(); idx_segment++) { + for (size_t idx_linestring = 0; idx_linestring < n_linestrings_per_lanelet; idx_linestring++) { + ls_points.clear(); + ls_points.reserve(msg.segments[idx_segment].linestrings[idx_linestring].poses.size()); + for (size_t id_pose = 0; + id_pose < msg.segments[idx_segment].linestrings[idx_linestring].poses.size(); + id_pose++) { + double p1p = + msg.segments[idx_segment].linestrings[idx_linestring].poses[id_pose].position.x; + double p2p = + msg.segments[idx_segment].linestrings[idx_linestring].poses[id_pose].position.y; + double p3p = + msg.segments[idx_segment].linestrings[idx_linestring].poses[id_pose].position.z; + + lanelet::Point3d p{lanelet::utils::getId(), p1p, p2p, p3p}; + ls_points.push_back(p); + } + + // Create a linestring from the collected points + lanelet::LineString3d linestring(lanelet::utils::getId(), ls_points); + la_linestrings[idx_linestring] = linestring; + } + + // One lanelet consists of 2 boundaries + lanelet::Lanelet lanelet(lanelet::utils::getId(), la_linestrings[0], la_linestrings[1]); + + out_lanelets.push_back(lanelet); + + // Add empty lanelet connection + out_lanelet_connections.push_back(lanelet_types::LaneletConnection()); + + // Set origin lanelet ID (and store it in translation map) + out_lanelet_connections[idx_segment].original_lanelet_id = msg.segments[idx_segment].id; + map_original_to_new[msg.segments[idx_segment].id] = idx_segment; + + // Get successor/neighbor lanelet information + // Write lanelet neighbors + for (std::size_t i = 0; i < msg.segments[idx_segment].neighboring_lanelet_id.size(); i++) { + out_lanelet_connections[idx_segment].neighbor_lanelet_ids.push_back( + msg.segments[idx_segment].neighboring_lanelet_id[i]); + } + + // Write lanelet successors + for (std::size_t i = 0; i < msg.segments[idx_segment].successor_lanelet_id.size(); i++) { + out_lanelet_connections[idx_segment].successor_lanelet_ids.push_back( + msg.segments[idx_segment].successor_lanelet_id[i]); + } + + // The goal_information is not needed in this context, we set it to true for now + out_lanelet_connections[idx_segment].goal_information = true; + } + + // Define lambda function to replace old id with a new one + auto ReplaceAndWarnIfNotFound = [&](auto & lanelet_ids) { + for (auto & lanelet_id : lanelet_ids) { + if (lanelet_id >= 0) { + auto it = map_original_to_new.find(lanelet_id); + if (it != map_original_to_new.end()) { + lanelet_id = it->second; + } else { + // Key %i seems to be not present in the provided lanelet. + } + } + } + }; + + // Update each entity in all lanelets + for (auto & lanelet_connection : out_lanelet_connections) { + ReplaceAndWarnIfNotFound(lanelet_connection.predecessor_lanelet_ids); + ReplaceAndWarnIfNotFound(lanelet_connection.successor_lanelet_ids); + ReplaceAndWarnIfNotFound(lanelet_connection.neighbor_lanelet_ids); + } + + // Fill predecessor field for each lanelet + this->CalculatePredecessors(out_lanelet_connections); + + return; +} + +void MissionPlannerNode::CalculatePredecessors( + std::vector & lanelet_connections) +{ + // Determine predecessor information from already known information + for (std::size_t id_lanelet = 0; id_lanelet < lanelet_connections.size(); id_lanelet++) { + // Write lanelet predecessors (which are the successors of their previous + // lanelets) + for (std::size_t n_successor = 0; + n_successor < lanelet_connections[id_lanelet].successor_lanelet_ids.size(); + n_successor++) { + // Check if current lanelet has a valid successor, otherwise end of + // current local environment model has been reached and all the + // predecessors have been written with the last iteration + if (lanelet_connections[id_lanelet].successor_lanelet_ids[n_successor] > -1) { + lanelet_connections[lanelet_connections[id_lanelet].successor_lanelet_ids[n_successor]] + .predecessor_lanelet_ids.push_back(id_lanelet); + } + } + } + + // Write -1 to lanelets which have no predecessors + for (lanelet_types::LaneletConnection lanelet_connection : lanelet_connections) { + if (lanelet_connection.predecessor_lanelet_ids.empty()) { + lanelet_connection.predecessor_lanelet_ids = {-1}; + } + } +} diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/test/gtest_main.cpp b/planning/mapless_architecture/src/autoware_local_mission_planner/test/gtest_main.cpp new file mode 100644 index 0000000000000..3eb86a362bb0d --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/test/gtest_main.cpp @@ -0,0 +1,54 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "gtest/gtest.h" +#include "include/test_mission_planner_core.hpp" +#include "rclcpp/rclcpp.hpp" + +TEST(MissionPlannerCore, CalculateDistanceBetweenPointAndLineString) +{ + EXPECT_EQ(TestCalculateDistanceBetweenPointAndLineString(), 0); +} + +TEST(MissionPlannerCore, GetPointOnLane) +{ + EXPECT_EQ(TestGetPointOnLane(), 0); +} + +TEST(MissionPlannerCore, RecenterGoalPoint) +{ + EXPECT_EQ(TestRecenterGoalpoint(), 0); +} + +TEST(MissionPlanner, IsOnGoalLane) +{ + EXPECT_EQ(TestIsOnGoalLane(), 0); +} + +TEST(MissionPlanner, CheckIfGoalPointShouldBeReset) +{ + EXPECT_EQ(TestCheckIfGoalPointShouldBeReset(), 0); +} + +TEST(MissionPlanner, CalculateLanes) +{ + EXPECT_EQ(TestCalculateLanes(), 0); +} + +TEST(MissionPlanner, CreateMarkerArray) +{ + EXPECT_EQ(TestCreateMarkerArray(), 0); +} + +TEST(MissionPlanner, CreateDrivingCorridor) +{ + EXPECT_EQ(TestCreateDrivingCorridor(), 0); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); + rclcpp::shutdown(); +} diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp b/planning/mapless_architecture/src/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp new file mode 100644 index 0000000000000..d5d8f0041bf75 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp @@ -0,0 +1,75 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef TEST_MISSION_PLANNER_CORE_HPP_ +#define TEST_MISSION_PLANNER_CORE_HPP_ + +#include "db_msgs/msg/lanelets_stamped.hpp" + +/** + * @brief Test distance between point and LineString calculation. + * + * This function tests CalculateDistanceBetweenPointAndLineString_() which + * calculates the distance between a point and a LineString. + * + * @return int: returns 0 on success + */ +int TestCalculateDistanceBetweenPointAndLineString(); + +/** + * @brief Test GetPointOnLane_() function. + * + * This function tests GetPointOnLane_() which + * returns a point on a given lane in a given distance (x axis). + * + * @return int: returns 0 on success + */ +int TestGetPointOnLane(); + +/** + * @brief Test RecenterGoalPoint() function. + * + * @return int: returns 0 on success + */ +int TestRecenterGoalpoint(); + +/** + * @brief Test IsOnGoalLane_() function. + * + * This function tests IsOnGoalLane_() which + * returns true if the vehicle is on the goal lane (defined by the goal point). + * + * @return int: returns 0 on success + */ +int TestIsOnGoalLane(); + +/** + * @brief Test CheckIfGoalPointShouldBeReset_() function. + * + * If the x value of the goal point is negative, the goal point should be reset. + * + * @return int: returns 0 on success + */ +int TestCheckIfGoalPointShouldBeReset(); + +/** + * @brief Test CalculateLanes_() function. + * + * @return int: returns 0 on success + */ +int TestCalculateLanes(); + +/** + * @brief Test CreateMarkerArray_() function. + * + * @return int: returns 0 on success + */ +int TestCreateMarkerArray(); + +/** + * @brief Test CreateDrivingCorridor_() function. + * + * @return int: returns 0 on success + */ +int TestCreateDrivingCorridor(); + +#endif // TEST_MISSION_PLANNER_CORE_HPP_ diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/src/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp new file mode 100644 index 0000000000000..2a7edb198967e --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -0,0 +1,653 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "gtest/gtest.h" +#include "lib_mission_planner/helper_functions.hpp" +#include "mission_planner_node.hpp" + +#include "db_msgs/msg/lanelets_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include +#include + +db_msgs::msg::LaneletsStamped CreateLanelets() +{ + // Local variables + const int n_lanelets = 3; + const int n_attribute_vecs = 1; + const int n_points = 2; + + // Fill lanelet2 message + db_msgs::msg::LaneletsStamped message; + std::vector lanelet_vec(n_lanelets); + message.lanelets = lanelet_vec; + + // Global position + geometry_msgs::msg::PoseStamped msg_global_pose; + msg_global_pose.header.frame_id = "base_link"; + + std::array linestring_vec; + std::vector attribute_vec(n_attribute_vecs); + message.lanelets[0].id = 0; + message.lanelets[0].linestrings = linestring_vec; + message.lanelets[0].attributes = attribute_vec; + message.lanelets[1].id = 1; + message.lanelets[1].linestrings = linestring_vec; + message.lanelets[1].attributes = attribute_vec; + message.lanelets[2].id = 2; + message.lanelets[2].linestrings = linestring_vec; + message.lanelets[2].attributes = attribute_vec; + + uint16_t id = 0; + std::vector points_vec(n_points); + message.lanelets[0].linestrings[0].id = id; + message.lanelets[0].linestrings[1].id = ++id; + message.lanelets[0].linestrings[0].points = points_vec; + message.lanelets[0].linestrings[1].points = points_vec; + message.lanelets[1].linestrings[0].id = ++id; + message.lanelets[1].linestrings[1].id = ++id; + message.lanelets[1].linestrings[0].points = points_vec; + message.lanelets[1].linestrings[1].points = points_vec; + message.lanelets[2].linestrings[0].id = ++id; + message.lanelets[2].linestrings[1].id = ++id; + message.lanelets[2].linestrings[0].points = points_vec; + message.lanelets[2].linestrings[1].points = points_vec; + + tf2::Quaternion quaternion; + + // Vehicle position in global cosy + message.pose.position.x = 0.0; + message.pose.position.y = 0.0; + quaternion.setRPY(0, 0, 0); + message.pose.orientation.x = quaternion.getX(); + message.pose.orientation.y = quaternion.getY(); + message.pose.orientation.z = quaternion.getZ(); + message.pose.orientation.w = quaternion.getW(); + + // Street layout in current local cosy = global cosy since vehicle position is + // at the origin in the global cosy + message.lanelets[0].linestrings[0].points[0].pose.position.x = -2.0; + message.lanelets[0].linestrings[0].points[0].pose.position.y = -0.5; + message.lanelets[0].linestrings[0].points[0].pose.position.z = 0.0; + message.lanelets[0].linestrings[0].points[1].pose.position.x = 10.0; + message.lanelets[0].linestrings[0].points[1].pose.position.y = -0.5; + message.lanelets[0].linestrings[0].points[1].pose.position.z = 0.0; + + message.lanelets[0].linestrings[1].points[0].pose.position.x = -2.0; + message.lanelets[0].linestrings[1].points[0].pose.position.y = 0.5; + message.lanelets[0].linestrings[1].points[0].pose.position.z = 0.0; + message.lanelets[0].linestrings[1].points[1].pose.position.x = 10.0; + message.lanelets[0].linestrings[1].points[1].pose.position.y = 0.5; + message.lanelets[0].linestrings[1].points[1].pose.position.z = 0.0; + + message.lanelets[1].linestrings[0].points[0].pose.position.x = 0.0; + message.lanelets[1].linestrings[0].points[0].pose.position.y = 1.0; + message.lanelets[1].linestrings[0].points[0].pose.position.z = 0.0; + message.lanelets[1].linestrings[0].points[1].pose.position.x = 10.0; + message.lanelets[1].linestrings[0].points[1].pose.position.y = 1.0; + message.lanelets[1].linestrings[0].points[1].pose.position.z = 0.0; + + message.lanelets[1].linestrings[1].points[0].pose.position.x = 0.0; + message.lanelets[1].linestrings[1].points[0].pose.position.y = 2.0; + message.lanelets[1].linestrings[1].points[0].pose.position.z = 0.0; + message.lanelets[1].linestrings[1].points[1].pose.position.x = 10.0; + message.lanelets[1].linestrings[1].points[1].pose.position.y = 2.0; + message.lanelets[1].linestrings[1].points[1].pose.position.z = 0.0; + + message.lanelets[2].linestrings[0].points[0].pose.position.x = 10.0; + message.lanelets[2].linestrings[0].points[0].pose.position.y = -0.5; + message.lanelets[2].linestrings[0].points[0].pose.position.z = 0.0; + message.lanelets[2].linestrings[0].points[1].pose.position.x = 20.0; + message.lanelets[2].linestrings[0].points[1].pose.position.y = -0.5; + message.lanelets[2].linestrings[0].points[1].pose.position.z = 0.0; + + message.lanelets[2].linestrings[1].points[0].pose.position.x = 10.0; + message.lanelets[2].linestrings[1].points[0].pose.position.y = 0.5; + message.lanelets[2].linestrings[1].points[0].pose.position.z = 0.0; + message.lanelets[2].linestrings[1].points[1].pose.position.x = 20.0; + message.lanelets[2].linestrings[1].points[1].pose.position.y = 0.5; + message.lanelets[2].linestrings[1].points[1].pose.position.z = 0.0; + + // Define connections + message.lanelets[0].neighboring_lanelet_id = {0, 1}; + message.lanelets[1].neighboring_lanelet_id = {0, 1}; + message.lanelets[2].neighboring_lanelet_id = {-1, -1}; + message.lanelets[0].successor_lanelet_id = {2}; + message.lanelets[1].successor_lanelet_id = {-1}; + message.lanelets[2].successor_lanelet_id = {-1}; + + return message; +} + +int TestCalculateDistanceBetweenPointAndLineString() +{ + // Create an example Linestring + lanelet::LineString2d linestring; + lanelet::Point2d p1(0, 1.0, + 0.0); // First argument is ID (set it to 0 for now) + linestring.push_back(p1); + lanelet::Point2d p2(0, 2.0, + 0.0); // First argument is ID (set it to 0 for now) + linestring.push_back(p2); + lanelet::Point2d p3(0, 3.0, + 0.0); // First argument is ID (set it to 0 for now) + linestring.push_back(p3); + + // Create an example point + lanelet::BasicPoint2d point(1.0, 1.0); + + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + // Run function + double distance = MissionPlanner.CalculateDistanceBetweenPointAndLineString_(linestring, point); + + // Check if distance is near to 1.0 (with allowed error of 0.001) + EXPECT_NEAR(distance, 1.0, 0.001); + + return 0; +} + +int TestGetPointOnLane() +{ + // Create some example lanelets + auto msg_lanelets = CreateLanelets(); + + // convert road model + LaneletConverter lanelet_converter; + std::vector lanelet_connections; + std::vector lanelets; + + lanelet_converter.ConvertInput2LaneletFormat(msg_lanelets, lanelets, lanelet_connections); + + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + // Get a point from the tested function which has the x value 3.0 and lies on + // the centerline of the lanelets + lanelet::BasicPoint2d point = MissionPlanner.GetPointOnLane_({0, 1}, 3.0, lanelets); + + // Check if x value of the point is near to 3.0 (with an allowed error of + // 0.001) + EXPECT_NEAR(point.x(), 3.0, 0.001); + + // Get a point from the tested function which has the x value 100.0 and lies + // on the centerline of the lanelets + point = MissionPlanner.GetPointOnLane_({0, 1}, 100.0, + lanelets); // Far away (100m) + + // Check if x value of the point is near to 10.0 (with an allowed error of + // 0.001) + EXPECT_NEAR(point.x(), 10.0, 0.001); + + return 0; +} + +int TestIsOnGoalLane() +{ + // Create some example lanelets + auto msg_road_model = CreateLanelets(); + // convert road model + LaneletConverter lanelet_converter; + std::vector lanelet_connections; + std::vector lanelets; + + lanelet_converter.ConvertInput2LaneletFormat(msg_road_model, lanelets, lanelet_connections); + + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + // Define a point with x = 1.0 and y = 0.0 + lanelet::BasicPoint2d point(1.0, 0.0); + + // Check if the point is on the lane: should be true + EXPECT_EQ(MissionPlanner.IsOnGoalLane_(0, point, lanelets, lanelet_connections), true); + + // Define a point with x = 100.0 and y = 100.0 + lanelet::BasicPoint2d point2(100.0, 100.0); + + // Check if the point is on the lane: should be false + EXPECT_EQ(MissionPlanner.IsOnGoalLane_(0, point2, lanelets, lanelet_connections), false); + + // Define a point with x = 15.0 and y = 0.0 + lanelet::BasicPoint2d point3(15.0, 0.0); + + // Check if the point is on the lane: should be true + EXPECT_EQ(MissionPlanner.IsOnGoalLane_(0, point3, lanelets, lanelet_connections), true); + + return 0; +} + +db_msgs::msg::LaneletsStamped GetTestRoadModelForRecenterTests() +{ + // local variables + const int n_lanelets = 2; + const int n_attribute_vecs = 1; + const int n_points = 2; + + // Fill lanelet2 message + db_msgs::msg::LaneletsStamped message; + std::vector lanelet_vec(n_lanelets); + message.lanelets = lanelet_vec; + + // global position + geometry_msgs::msg::PoseStamped msg_global_pose; + msg_global_pose.header.frame_id = "base_link"; + + std::array linestring_vec; + std::vector attribute_vec(n_attribute_vecs); + message.lanelets[0].id = 0; + message.lanelets[0].linestrings = linestring_vec; + message.lanelets[0].attributes = attribute_vec; + message.lanelets[1].id = 1; + message.lanelets[1].linestrings = linestring_vec; + message.lanelets[1].attributes = attribute_vec; + + uint16_t id = 0; + std::vector points_vec(n_points); + message.lanelets[0].linestrings[0].id = id; + message.lanelets[0].linestrings[1].id = ++id; + message.lanelets[0].linestrings[0].points = points_vec; + message.lanelets[0].linestrings[1].points = points_vec; + message.lanelets[1].linestrings[0].id = ++id; + message.lanelets[1].linestrings[1].id = ++id; + message.lanelets[1].linestrings[0].points = points_vec; + message.lanelets[1].linestrings[1].points = points_vec; + + tf2::Quaternion quaternion; + + // vehicle position in global cosy + message.pose.position.x = 0.0; + message.pose.position.y = 0.0; + quaternion.setRPY(0, 0, 0); + message.pose.orientation.x = quaternion.getX(); + message.pose.orientation.y = quaternion.getY(); + message.pose.orientation.z = quaternion.getZ(); + message.pose.orientation.w = quaternion.getW(); + + // street layout in current local cosy = global cosy since vehicle position is + // at the origin in the global cosy + message.lanelets[0].linestrings[0].points[0].pose.position.x = 0.0; + message.lanelets[0].linestrings[0].points[0].pose.position.y = -0.5; + message.lanelets[0].linestrings[0].points[0].pose.position.z = 0.0; + message.lanelets[0].linestrings[0].points[1].pose.position.x = 10.0; + message.lanelets[0].linestrings[0].points[1].pose.position.y = -0.5; + message.lanelets[0].linestrings[0].points[1].pose.position.z = 0.0; + + message.lanelets[0].linestrings[1].points[0].pose.position.x = 0.0; + message.lanelets[0].linestrings[1].points[0].pose.position.y = 0.5; + message.lanelets[0].linestrings[1].points[0].pose.position.z = 0.0; + message.lanelets[0].linestrings[1].points[1].pose.position.x = 10.0; + message.lanelets[0].linestrings[1].points[1].pose.position.y = 0.5; + message.lanelets[0].linestrings[1].points[1].pose.position.z = 0.0; + + message.lanelets[1].linestrings[0].points[0].pose.position.x = 0.0; + message.lanelets[1].linestrings[0].points[0].pose.position.y = 1.0; + message.lanelets[1].linestrings[0].points[0].pose.position.z = 0.0; + message.lanelets[1].linestrings[0].points[1].pose.position.x = 10.0; + message.lanelets[1].linestrings[0].points[1].pose.position.y = 1.0; + message.lanelets[1].linestrings[0].points[1].pose.position.z = 0.0; + + message.lanelets[1].linestrings[1].points[0].pose.position.x = 0.0; + message.lanelets[1].linestrings[1].points[0].pose.position.y = 2.0; + message.lanelets[1].linestrings[1].points[0].pose.position.z = 0.0; + message.lanelets[1].linestrings[1].points[1].pose.position.x = 10.0; + message.lanelets[1].linestrings[1].points[1].pose.position.y = 2.0; + message.lanelets[1].linestrings[1].points[1].pose.position.z = 0.0; + + int32_t neigh_1 = 1; + std::vector neighboring_ids = {neigh_1}; + message.lanelets[0].neighboring_lanelet_id = neighboring_ids; + std::vector neighboring_ids_2 = {}; + message.lanelets[1].neighboring_lanelet_id = neighboring_ids_2; + + return message; +} + +int TestRecenterGoalpoint() +{ + // Create a mission planner + MissionPlannerNode mission_planner = MissionPlannerNode(); + + // Get a local road model for testing + db_msgs::msg::LaneletsStamped local_road_model = GetTestRoadModelForRecenterTests(); + + LaneletConverter lanelet_converter; + + // Used for the output + std::vector lanelet_connections; + std::vector converted_lanelets; + + lanelet_converter.ConvertInput2LaneletFormat( + local_road_model, converted_lanelets, lanelet_connections); + + // TEST 1: point on centerline of origin lanelet + // define a test point and re-center onto its centerline + lanelet::BasicPoint2d goal_point(0.0, 0.0); + lanelet::BasicPoint2d goal_point_recentered = + mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + + EXPECT_NEAR(goal_point_recentered.x(), 0.0, 1e-5); + EXPECT_NEAR(goal_point_recentered.y(), 0.0, 1e-5); + + // TEST 2: point left to centerline of origin lanelet + // define a test point which is not on the centerline + goal_point.x() = 1.0; + goal_point.y() = 0.25; + goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + + // expect re-centered point to lie on y coordinate 0 + EXPECT_NEAR(goal_point_recentered.x(), goal_point.x(), 1e-5); + EXPECT_NEAR(goal_point_recentered.y(), 0.0, 1e-5); + + // TEST 3: point right to centerline of origin lanelet + // define a test point which is not on the centerline + goal_point.x() = 8.0; + goal_point.y() = -0.2; + goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + + // expect re-centered point to lie on y coordinate 0 but with x coord equal to + // goal_point (removes lateral error/noise) + EXPECT_NEAR(goal_point_recentered.x(), goal_point.x(), 1e-5); + EXPECT_NEAR(goal_point_recentered.y(), 0.0, 1e-5); + + // TEST 4: point left of centerline of neighboring lanelet -> tests if correct + // lanelet is selected for re-centering define a test point which is not on + // the centerline + goal_point.x() = 2.0; + goal_point.y() = 1.75; + goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + + // expect re-centered point to lie on y coordinate 0 but with x coord equal to + // goal_point (removes lateral error/noise) + EXPECT_NEAR(goal_point_recentered.x(), goal_point.x(), 1e-5); + EXPECT_NEAR(goal_point_recentered.y(), 1.5, 1e-5); + + // TEST 5: point outside of local road model, goal point should remain + // untouched + goal_point.x() = 11.0; + goal_point.y() = -2.0; + goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + + EXPECT_EQ(goal_point_recentered.x(), goal_point.x()); + EXPECT_EQ(goal_point_recentered.y(), goal_point.y()); + + return 0; +} + +int TestCheckIfGoalPointShouldBeReset() +{ + // Create some example lanelets + auto msg = CreateLanelets(); + + // Convert message + mission_planner_messages::msg::RoadSegments road_segments = + lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg); + mission_planner_messages::msg::LocalMap local_map; + local_map.road_segments = road_segments; + + // Convert road model + LaneletConverter lanelet_converter; + std::vector lanelet_connections; + std::vector lanelets; + + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); + + // Compute available lanes + MissionPlanner.CallbackLocalMapMessages_(local_map); + + // TEST 1: check if goal point is reset in non-default mission + // Define a goal point with negative x value + lanelet::BasicPoint2d point(-1.0, 0.0); + MissionPlanner.goal_point(point); + + // set a non-default mission to make the goal point reset work + mission_planner_messages::msg::Mission mission_msg; + mission_msg.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_LEFT; + MissionPlanner.CallbackMissionMessages_(mission_msg); + + // Call function which is tested + MissionPlanner.CheckIfGoalPointShouldBeReset_(lanelets, lanelet_connections); + + // Check if the goal point is reset + EXPECT_EQ( + MissionPlanner.goal_point().x(), + 10); // Projection is to x = 10, since this is the highest x value + // on the right neighbor lane! + + // TEST 2: check if goal point reset is skipped in default mission + MissionPlanner.goal_point(point); + + // set a non-default mission to make the goal point reset work + mission_msg.mission_type = mission_planner_messages::msg::Mission::LANE_KEEP; + MissionPlanner.CallbackMissionMessages_(mission_msg); + + // Call function which is tested + MissionPlanner.CheckIfGoalPointShouldBeReset_(lanelets, lanelet_connections); + + // Check if the goal point is reset + EXPECT_EQ( + MissionPlanner.goal_point().x(), + point.x()); // goal point should equal the input point since goal point + // is not reset in the default mission (ego lane following) + + return 0; +} + +std::tuple, std::vector> +CreateLane() +{ + // Local variables + const int n_lanelets = 2; + const int n_attribute_vecs = 1; + const int n_points = 2; + + // Fill lanelet2 message + db_msgs::msg::LaneletsStamped message; + std::vector lanelet_vec(n_lanelets); + message.lanelets = lanelet_vec; + + // Global position + geometry_msgs::msg::PoseStamped msg_global_pose; + msg_global_pose.header.frame_id = "base_link"; + + std::array linestring_vec; + std::vector attribute_vec(n_attribute_vecs); + message.lanelets[0].id = 0; + message.lanelets[0].linestrings = linestring_vec; + message.lanelets[0].attributes = attribute_vec; + message.lanelets[1].id = 1; + message.lanelets[1].linestrings = linestring_vec; + message.lanelets[1].attributes = attribute_vec; + + uint16_t id = 0; + std::vector points_vec(n_points); + message.lanelets[0].linestrings[0].id = id; + message.lanelets[0].linestrings[1].id = ++id; + message.lanelets[0].linestrings[0].points = points_vec; + message.lanelets[0].linestrings[1].points = points_vec; + message.lanelets[1].linestrings[0].id = ++id; + message.lanelets[1].linestrings[1].id = ++id; + message.lanelets[1].linestrings[0].points = points_vec; + message.lanelets[1].linestrings[1].points = points_vec; + + tf2::Quaternion quaternion; + + // Vehicle position in global cosy + message.pose.position.x = 0.0; + message.pose.position.y = 0.0; + quaternion.setRPY(0, 0, 0); + message.pose.orientation.x = quaternion.getX(); + message.pose.orientation.y = quaternion.getY(); + message.pose.orientation.z = quaternion.getZ(); + message.pose.orientation.w = quaternion.getW(); + + // Street layout in current local cosy = global cosy since vehicle position is + // at the origin in the global cosy + message.lanelets[0].linestrings[0].points[0].pose.position.x = -2.0; + message.lanelets[0].linestrings[0].points[0].pose.position.y = -0.5; + message.lanelets[0].linestrings[0].points[0].pose.position.z = 0.0; + message.lanelets[0].linestrings[0].points[1].pose.position.x = 10.0; + message.lanelets[0].linestrings[0].points[1].pose.position.y = -0.5; + message.lanelets[0].linestrings[0].points[1].pose.position.z = 0.0; + + message.lanelets[0].linestrings[1].points[0].pose.position.x = -2.0; + message.lanelets[0].linestrings[1].points[0].pose.position.y = 0.5; + message.lanelets[0].linestrings[1].points[0].pose.position.z = 0.0; + message.lanelets[0].linestrings[1].points[1].pose.position.x = 10.0; + message.lanelets[0].linestrings[1].points[1].pose.position.y = 0.5; + message.lanelets[0].linestrings[1].points[1].pose.position.z = 0.0; + + message.lanelets[1].linestrings[0].points[0].pose.position.x = 10.0; + message.lanelets[1].linestrings[0].points[0].pose.position.y = -0.5; + message.lanelets[1].linestrings[0].points[0].pose.position.z = 0.0; + message.lanelets[1].linestrings[0].points[1].pose.position.x = 20.0; + message.lanelets[1].linestrings[0].points[1].pose.position.y = -0.5; + message.lanelets[1].linestrings[0].points[1].pose.position.z = 0.0; + + message.lanelets[1].linestrings[1].points[0].pose.position.x = 10.0; + message.lanelets[1].linestrings[1].points[0].pose.position.y = 0.5; + message.lanelets[1].linestrings[1].points[0].pose.position.z = 0.0; + message.lanelets[1].linestrings[1].points[1].pose.position.x = 20.0; + message.lanelets[1].linestrings[1].points[1].pose.position.y = 0.5; + message.lanelets[1].linestrings[1].points[1].pose.position.z = 0.0; + + // Define connections + message.lanelets[0].successor_lanelet_id = {1}; + message.lanelets[0].neighboring_lanelet_id = {-1, -1}; + message.lanelets[1].successor_lanelet_id = {-1}; + message.lanelets[1].neighboring_lanelet_id = {-1, -1}; + + // Get converter + LaneletConverter lanelet_converter; + + // Output + std::vector lanelet_connections; + std::vector converted_lanelets; + + lanelet_converter.ConvertInput2LaneletFormat(message, converted_lanelets, lanelet_connections); + return std::make_tuple(converted_lanelets, lanelet_connections); +} + +int TestCalculateLanes() +{ + // Create some example lanelets: Two lanelets 0 and 1, 1 is the successor of + // 0, the ego lanelet is 0 + auto tuple = CreateLane(); + std::vector lanelets = std::get<0>(tuple); + std::vector lanelet_connections = std::get<1>(tuple); + + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + // Call function which is tested + Lanes result = MissionPlanner.CalculateLanes_(lanelets, lanelet_connections); + + // Get lanes + std::vector ego_lane_idx = result.ego; + std::vector left_lane_idx = result.left[0]; + std::vector right_lane_idx = result.right[0]; + + // Check if left lane is empty + EXPECT_EQ(left_lane_idx.empty(), true); // Expect empty lane + + // Check if right lane is empty + EXPECT_EQ(right_lane_idx.empty(), true); // Expect empty lane + + // Check if ego lane is correct + EXPECT_EQ( + ego_lane_idx[0], + 0); // Expect ego lane = {0, 1} (1 is index of successor of ego lanelet) + + EXPECT_EQ( + ego_lane_idx[1], + 1); // Expect ego lane = {0, 1} (1 is index of successor of ego lanelet) + + return 0; +} + +int TestCreateMarkerArray() +{ + // Create some example lanelets + auto tuple = CreateLane(); + std::vector lanelets = std::get<0>(tuple); + std::vector lanelet_connections = std::get<1>(tuple); + + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + // Create empty message + mission_planner_messages::msg::RoadSegments message; + + // Calculate centerlines, left and right bounds + std::vector centerlines; + std::vector left_bounds; + std::vector right_bounds; + + for (const lanelet::Lanelet & l : lanelets) { + auto centerline = l.centerline(); + auto bound_left = l.leftBound(); + auto bound_right = l.rightBound(); + + centerlines.push_back(centerline); + left_bounds.push_back(bound_left); + right_bounds.push_back(bound_right); + } + + // Call function which is tested + auto marker_array = + MissionPlanner.CreateMarkerArray_(centerlines, left_bounds, right_bounds, message); + + // Check if marker_array is empty + EXPECT_EQ(marker_array.markers.empty(), + false); // Expect not empty marker_array + + // Check first point of marker array (should be first point on the centerline) + EXPECT_EQ(marker_array.markers[0].points[0].x, + -2.0); // Expect -2.0 for x + + // Check first point of marker array (should be first point on the centerline) + EXPECT_EQ(marker_array.markers[0].points[0].y, + 0.0); // Expect 0.0 for y + + return 0; +} + +int TestCreateDrivingCorridor() +{ + // Create some example lanelets + auto tuple = CreateLane(); + std::vector lanelets = std::get<0>(tuple); + std::vector lanelet_connections = std::get<1>(tuple); + + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + // Call function which is tested + mission_planner_messages::msg::DrivingCorridor driving_corridor = + MissionPlanner.CreateDrivingCorridor_({0, 1}, lanelets); + + // Check if x value of first point in centerline is -2.0 + EXPECT_EQ(driving_corridor.centerline[0].x, -2.0); + + // Check if y value of first point in centerline is 0.0 + EXPECT_EQ(driving_corridor.centerline[0].y, 0.0); + + // Check if x value of first point in left bound is -2.0 + EXPECT_EQ(driving_corridor.bound_left[0].x, -2.0); + + // Check if y value of first point in left bound is -0.5 + EXPECT_EQ(driving_corridor.bound_left[0].y, -0.5); + + // Check if x value of first point in right bound is -2.0 + EXPECT_EQ(driving_corridor.bound_right[0].x, -2.0); + + // Check if y value of first point in right bound is 0.5 + EXPECT_EQ(driving_corridor.bound_right[0].y, 0.5); + + return 0; +} diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/CMakeLists.txt b/planning/mapless_architecture/src/autoware_local_road_provider/CMakeLists.txt new file mode 100644 index 0000000000000..77f141fcc9d44 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.8) +project(local_road_provider) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# --- FIND DEPENDENCIES --- +find_package(autoware_cmake REQUIRED) # automatically find dependencies +ament_auto_find_build_dependencies() +autoware_package() + +# build executables +ament_auto_add_executable(${PROJECT_NAME} + src/local_road_provider_main.cpp + src/local_road_provider_node.cpp) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# install executable(s) +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# install launchfile(s) [all within the "launch" folder] +install(DIRECTORY +launch +DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +# configure clang format +set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/Readme.md b/planning/mapless_architecture/src/autoware_local_road_provider/Readme.md new file mode 100644 index 0000000000000..147f05155c78f --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/Readme.md @@ -0,0 +1,3 @@ +# Local Road Provider Node + +This node converts the db_msgs::msg::LaneletsStamped message into a mission_planner_messages::msg::RoadSegments message right now. More functionality can be added later. diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/include/local_road_provider_node.hpp b/planning/mapless_architecture/src/autoware_local_road_provider/include/local_road_provider_node.hpp new file mode 100644 index 0000000000000..e3e0f82f98dc4 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/include/local_road_provider_node.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef LOCAL_ROAD_PROVIDER_NODE_HPP_ +#define LOCAL_ROAD_PROVIDER_NODE_HPP_ + +#include "mission_planner_messages/msg/road_segments.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "db_msgs/msg/lanelets_stamped.hpp" + +/** + * Node for the Local Road Provider. + */ +class LocalRoadProviderNode : public rclcpp::Node +{ +public: + /** + * @brief Constructor for the LocalRoadProviderNode class. + * + * Initializes the publisher and subscriber with appropriate topics and QoS + * settings. + */ + LocalRoadProviderNode(); + +private: + // ########################################################################### + // # PRIVATE PROCESSING METHODS + // ########################################################################### + + /** + * @brief The callback for the LaneletsStamped messages. + * + * @param msg The db_msgs::msg::LaneletsStamped message. + */ + void CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg); + + // ########################################################################### + // # PRIVATE VARIABLES + // ########################################################################### + // Declare ROS2 publisher and subscriber + + rclcpp::Publisher::SharedPtr road_publisher_; + + rclcpp::Subscription::SharedPtr lanelets_subscriber_; +}; + +#endif // LOCAL_ROAD_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/launch/local_road_provider.launch.py b/planning/mapless_architecture/src/autoware_local_road_provider/launch/local_road_provider.launch.py new file mode 100644 index 0000000000000..530c57745a110 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/launch/local_road_provider.launch.py @@ -0,0 +1,27 @@ +# Copyright 2024 driveblocks GmbH +# driveblocks proprietary license +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + # local_road_provider executable + Node( + package="local_road_provider", + executable="local_road_provider", + name="local_road_provider", + namespace="mission_planner", + remappings=[ + ( + "local_road_provider_node/output/road_segments", + "local_road_provider_node/output/road_segments", + ), + ("local_road_provider_node/input/lanelets", "/env/output/driving_corridors"), + ], + parameters=[], + output="screen", + ), + ] + ) diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/package.xml b/planning/mapless_architecture/src/autoware_local_road_provider/package.xml new file mode 100644 index 0000000000000..67e6211a2d361 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/package.xml @@ -0,0 +1,25 @@ + + + + local_road_provider + 0.0.1 + local_road_provider + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + db_msgs + lib_mission_planner + mission_planner_messages + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_main.cpp b/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_main.cpp new file mode 100644 index 0000000000000..00ddfe6eb9584 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_main.cpp @@ -0,0 +1,18 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "local_road_provider_node.hpp" + +#include + +#include + +int main(int argc, char * argv[]) +{ + RCLCPP_INFO(rclcpp::get_logger("local_road_provider"), "Launching Local Road Provider node..."); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_node.cpp new file mode 100644 index 0000000000000..1f0e645826b89 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_node.cpp @@ -0,0 +1,36 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "local_road_provider_node.hpp" + +#include "lib_mission_planner/helper_functions.hpp" +#include "rclcpp/rclcpp.hpp" + +using std::placeholders::_1; + +LocalRoadProviderNode::LocalRoadProviderNode() : Node("local_road_provider_node") +{ + // Set quality of service to best effort (if transmission fails, do not try to + // resend but rather use new sensor data) + // the history_depth is set to 1 (message queue size) + auto qos = rclcpp::QoS(1); + qos.best_effort(); + + // Initialize publisher for road segments + road_publisher_ = this->create_publisher( + "local_road_provider_node/output/road_segments", 1); + + // Initialize subscriber to lanelets stamped messages + lanelets_subscriber_ = this->create_subscription( + "local_road_provider_node/input/lanelets", qos, + std::bind(&LocalRoadProviderNode::CallbackLaneletsMessages_, this, _1)); +} + +void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg) +{ + mission_planner_messages::msg::RoadSegments road_segments = + lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg); + + // Publish the RoadSegments message + road_publisher_->publish(road_segments); +} diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/test/gtest_main.cpp b/planning/mapless_architecture/src/autoware_local_road_provider/test/gtest_main.cpp new file mode 100644 index 0000000000000..d885ea084b1a3 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_local_road_provider/test/gtest_main.cpp @@ -0,0 +1,9 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "gtest/gtest.h" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/CMakeLists.txt b/planning/mapless_architecture/src/autoware_mission_lane_converter/CMakeLists.txt new file mode 100644 index 0000000000000..c6d4e8f629e51 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(mission_lane_converter) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# --- FIND DEPENDENCIES --- +find_package(autoware_cmake REQUIRED) # automatically find dependencies +ament_auto_find_build_dependencies() +autoware_package() + +# build executables +ament_auto_add_executable(${PROJECT_NAME} + src/main_mission_lane_converter.cpp + src/mission_lane_converter_node.cpp) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# install executable(s) +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# install launchfile(s)/param file(s) +install(DIRECTORY + launch + param + DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +# configure clang format +set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp + test/src/test_mission_planner_converter.cpp + src/mission_lane_converter_node.cpp) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/Readme.md b/planning/mapless_architecture/src/autoware_mission_lane_converter/Readme.md new file mode 100644 index 0000000000000..df9b842f28ea1 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/Readme.md @@ -0,0 +1,6 @@ +# Mission Lane Converter + +Converts the selected mission lane to an autoware trajectory. + +## Build +In order to build the code, clone the TIER IV version of the autoware ROS messages from https://github.com/tier4/autoware_auto_msgs/tree/tier4/main and include in your build. diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp b/planning/mapless_architecture/src/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp new file mode 100644 index 0000000000000..47b1052083e48 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp @@ -0,0 +1,179 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef MISSION_LANE_CONVERTER_NODE_HPP_ +#define MISSION_LANE_CONVERTER_NODE_HPP_ + +#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include + +/** + * Node to convert the mission lane to an autoware trajectory type. + */ +class MissionLaneConverterNode : public rclcpp::Node +{ +public: + MissionLaneConverterNode(); + + // ########################################################################### + // # PUBLIC METHODS + // ########################################################################### + + /** + * @brief Converts the mission message into a reference trajectory which is + * forwarded to a local trajectory planner for refinement. + * + * @param msg The mission lanes + * @return std::tuple + */ + std::tuple< + autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + ConvertMissionToTrajectory(const mission_planner_messages::msg::MissionLanesStamped & msg); + +private: + // ########################################################################### + // # PRIVATE PROCESSING METHODS + // ########################################################################### + + /** + * @brief Computes a trajectory based on the mission planner input. + * + * @param msg Mission lanes from the mission planner module + */ + void MissionLanesCallback_(const mission_planner_messages::msg::MissionLanesStamped & msg); + + /** + * @brief Adds a trajectory point to the pre-allocated ROS message. + * + * @param trj_msg The pre-allocated trajectory message + * @param x The x position of the point to be added + * @param y The y position of the point to be added + * @param v_x The longitudinal velocity of the point to be added + */ + void AddTrajectoryPoint_( + autoware_auto_planning_msgs::msg::Trajectory & trj_msg, const double x, const double y, + const double v_x); + + /** + * @brief Adds a visualization point to the pre-allocated ROS message. + * + * @param trj_vis The pre-allocated visualization message + * @param x The x position of the marker to be added + * @param y The y position of the marker to be added + * @param v_x The id of the marker to be added + */ + void AddPointVisualizationMarker_( + visualization_msgs::msg::Marker & trj_vis, const double x, const double y, const int id_marker); + + /** + * @brief Computes and adds a heading information to the pre-allocated input + * trajectory based on the x/y positions given in the input argument + * + * @param trj_msg + */ + void AddHeadingToTrajectory_(autoware_auto_planning_msgs::msg::Trajectory & trj_msg); + + /** + * @brief Timed callback which shall be executed until a first valid local + * road model (where the vehicle can be located on) could have been computed + * once. This can be considered to be a workaround during startup of the local + * environment generation which starts in a certain distance in front of the + * vehicle. + * + */ + void TimedStartupTrajectoryCallback(); + + void CreatePathBound_( + std::vector & bound_path, visualization_msgs::msg::Marker & path_vis, + const std::vector & bound_mission_lane, const int id_marker); + + void AddPathPoint_( + autoware_auto_planning_msgs::msg::Path & pth_msg, const double x, const double y, + const double v_x); + + void CreateMotionPlannerInput_( + autoware_auto_planning_msgs::msg::Trajectory & trj_msg, + autoware_auto_planning_msgs::msg::Path & path_msg, visualization_msgs::msg::Marker & trj_vis, + visualization_msgs::msg::Marker & path_vis, + const std::vector & centerline_mission_lane); + + /** + * @brief Callback to store the most recent odometry update. + * + * @param msg Odometry input message + */ + void CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg); + + /** + * @brief Template to transform both Autoware::Path and Autoware::Trajectory into a global map + * frame. + * + * @tparam T autoware_auto_planning_msgs::msg::Path, autoware_auto_planning_msgs::msg::Trajectory + * @param input Input ROS message which content must be transformed into the global map frame + * @return T Same as input message with the content being valid in the global map + */ + template + T TransformToGlobalFrame(const T & input); + + /** + * @brief Visualization helper for the global trajectory + * + * @param trj_msg Trajectory in global map frame + * @return visualization_msgs::msg::Marker + */ + visualization_msgs::msg::Marker GetGlobalTrjVisualization_( + const autoware_auto_planning_msgs::msg::Trajectory & trj_msg); + + // ########################################################################### + // # PRIVATE VARIABLES + // ########################################################################### + // Declare ROS2 publisher and subscriber + + rclcpp::Subscription::SharedPtr odom_subscriber_; + + rclcpp::Subscription::SharedPtr + mission_lane_subscriber_; + + rclcpp::Publisher::SharedPtr trajectory_publisher_, + trajectory_publisher_global_; + + rclcpp::Publisher::SharedPtr path_publisher_, + path_publisher_global_; + + rclcpp::Publisher::SharedPtr vis_trajectory_publisher_, + vis_trajectory_publisher_global_, vis_odometry_publisher_global_; + + rclcpp::Publisher::SharedPtr vis_path_publisher_; + + rclcpp::Publisher::SharedPtr publisher_; + + rclcpp::TimerBase::SharedPtr timer_; + + // switch to print a warning about wrongly configured odometry frames + bool b_global_odometry_deprecation_warning_ = false; + bool received_motion_update_once_ = false; + // store initial and last available odom messages + nav_msgs::msg::Odometry last_odom_msg_, initial_odom_msg_; + + // workaround to start the vehicle driving into the computed local road + // model + bool mission_lanes_available_once_ = false; + + // ros parameters + float target_speed_; +}; + +#endif // MISSION_LANE_CONVERTER_NODE_HPP_ diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py b/planning/mapless_architecture/src/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py new file mode 100644 index 0000000000000..74c39a1884f71 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py @@ -0,0 +1,67 @@ +# Copyright 2024 driveblocks GmbH +# driveblocks proprietary license +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + mission_lane_converter_param_file = os.path.join( + get_package_share_directory("mission_lane_converter"), + "param", + "mission_lane_converter_default.yaml", + ) + + mission_lane_converter_param = DeclareLaunchArgument( + "mission_lane_converter_param_file", + default_value=mission_lane_converter_param_file, + description="Path to config file for the mission lane converter.", + ) + + return LaunchDescription( + [ + # mission lane converter parameter + mission_lane_converter_param, + # mission lane converter executable + Node( + package="mission_lane_converter", + executable="mission_lane_converter", + name="mission_lane_converter", + namespace="mission_planner", + remappings=[ + ( + "mission_lane_converter/input/mission_lanes", + "mission_planner_node/output/mission_lanes_stamped", + ), + ( + "mission_lane_converter/input/odometry", + "/awsim/ground_truth/localization/kinematic_state", + ), + ( + "mission_lane_converter/output/trajectory", + "/planning/scenario_planning/local_trajectory", + ), + ( + "mission_lane_converter/output/global_trajectory", + "/planning/scenario_planning/trajectory", + ), + ( + "mission_lane_converter/output/path", + "mission_lane_converter/output/local_path", + ), + ( + "mission_lane_converter/output/global_path", + "mission_lane_converter/output/global_path", + ), + ], + parameters=[ + LaunchConfiguration("mission_lane_converter_param_file"), + ], + output="screen", + ), + ] + ) diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/package.xml b/planning/mapless_architecture/src/autoware_mission_lane_converter/package.xml new file mode 100644 index 0000000000000..4942e7b8dce5e --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/package.xml @@ -0,0 +1,28 @@ + + + + mission_lane_converter + 0.0.1 + Mission Lane Converter + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + autoware_auto_planning_msgs + geometry_msgs + lib_mission_planner + mission_planner_messages + rclcpp + tf2_geometry_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml b/planning/mapless_architecture/src/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml new file mode 100644 index 0000000000000..fd3a755e351d0 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml @@ -0,0 +1,4 @@ +/mission_planner: + mission_lane_converter: + ros__parameters: + target_speed: 3.2 # [mps] constant target speed of the mission trajectories diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp b/planning/mapless_architecture/src/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp new file mode 100644 index 0000000000000..c3a4ce80602d7 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp @@ -0,0 +1,18 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "mission_lane_converter_node.hpp" +#include "rclcpp/rclcpp.hpp" + +#include + +int main(int argc, char * argv[]) +{ + RCLCPP_INFO( + rclcpp::get_logger("mission_planner_converter_node"), + "Launching mission lane converter node..."); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/src/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp new file mode 100644 index 0000000000000..00f7f2456d56b --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -0,0 +1,611 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "mission_lane_converter_node.hpp" + +#include "lib_mission_planner/helper_functions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include + +#include +#include + +using std::placeholders::_1; +using namespace lib_mission_planner; + +MissionLaneConverterNode::MissionLaneConverterNode() : Node("mission_lane_converter_node") +{ + // Set quality of service to best effort (if transmission fails, do not try to + // resend but rather use new sensor data) + // the history_depth is set to 1 (message queue size) + auto qos_best_effort = rclcpp::QoS(1); + qos_best_effort.best_effort(); + + auto qos_reliability = rclcpp::QoS(1); + qos_reliability.reliability(); + + odom_subscriber_ = this->create_subscription( + "mission_lane_converter/input/odometry", qos_best_effort, + std::bind(&MissionLaneConverterNode::CallbackOdometryMessages_, this, _1)); + + mission_lane_subscriber_ = + this->create_subscription( + "mission_lane_converter/input/mission_lanes", qos_best_effort, + std::bind(&MissionLaneConverterNode::MissionLanesCallback_, this, _1)); + + // Initialize publisher + trajectory_publisher_ = this->create_publisher( + "mission_lane_converter/output/trajectory", qos_reliability); + + trajectory_publisher_global_ = + this->create_publisher( + "mission_lane_converter/output/global_trajectory", qos_reliability); + + // artificial publisher to test the trajectory generation + publisher_ = this->create_publisher( + "mission_lane_converter/output/global_trajectory", qos_reliability); + + // path publisher + path_publisher_ = create_publisher( + "mission_lane_converter/output/path", qos_reliability); + + path_publisher_global_ = create_publisher( + "mission_lane_converter/output/global_path", qos_reliability); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&MissionLaneConverterNode::TimedStartupTrajectoryCallback, this)); + + vis_trajectory_publisher_ = this->create_publisher( + "mission_lane_converter/output/vis_trajectory", qos_best_effort); + + vis_trajectory_publisher_global_ = this->create_publisher( + "mission_lane_converter/output/vis_global_trajectory", qos_best_effort); + + vis_path_publisher_ = this->create_publisher( + "mission_lane_converter/output/vis_path", qos_best_effort); + + vis_odometry_publisher_global_ = this->create_publisher( + "mission_lane_converter/output/vis_global_odometry", qos_best_effort); + + // ros parameters (will be overwritten by external param file if exists) + target_speed_ = declare_parameter("target_speed", 3.0); + RCLCPP_INFO(this->get_logger(), "Target speed set to: %.2f", target_speed_); +} + +void MissionLaneConverterNode::TimedStartupTrajectoryCallback() +{ + if (!mission_lanes_available_once_) { + // empty trajectory for controller + autoware_auto_planning_msgs::msg::Trajectory trj_msg = + autoware_auto_planning_msgs::msg::Trajectory(); + + // frame id must be "map" for Autoware controller + trj_msg.header.frame_id = "map"; + trj_msg.header.stamp = rclcpp::Node::now(); + + for (int idx_point = 0; idx_point < 100; idx_point++) { + const double x = -5.0 + idx_point * 1.0; + const double y = 0.0; + const double v_x = target_speed_; + + AddTrajectoryPoint_(trj_msg, x, y, v_x); + } + + // heading in trajectory path will be overwritten + this->AddHeadingToTrajectory_(trj_msg); + + trj_msg = TransformToGlobalFrame(trj_msg); + + publisher_->publish(trj_msg); + } +} + +void MissionLaneConverterNode::MissionLanesCallback_( + const mission_planner_messages::msg::MissionLanesStamped & msg_mission) +{ + // FIXME: workaround to get the vehicle driving in autonomous mode until the + // environment model is available + if (msg_mission.ego_lane.centerline.size() == 0) { + // do not continue to publish empty trajectory + if (mission_lanes_available_once_) { + // do only print warning if full mission lane was already available once + RCLCPP_WARN(this->get_logger(), "Received empty ego mission lane, aborting conversion!"); + } + return; + } else { + mission_lanes_available_once_ = true; + } + + std::tuple< + autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + mission_to_trj = ConvertMissionToTrajectory(msg_mission); + + autoware_auto_planning_msgs::msg::Trajectory trj_msg = std::get<0>(mission_to_trj); + visualization_msgs::msg::Marker trj_vis = std::get<1>(mission_to_trj); + autoware_auto_planning_msgs::msg::Path path_msg = std::get<2>(mission_to_trj); + visualization_msgs::msg::MarkerArray path_area = std::get<3>(mission_to_trj); + + autoware_auto_planning_msgs::msg::Trajectory trj_msg_global = + TransformToGlobalFrame(trj_msg); + autoware_auto_planning_msgs::msg::Path path_msg_global = + TransformToGlobalFrame(path_msg); + + visualization_msgs::msg::Marker trj_vis_global = GetGlobalTrjVisualization_(trj_msg_global); + vis_trajectory_publisher_global_->publish(trj_vis_global); + + // publish trajectory to motion planner + trajectory_publisher_->publish(trj_msg); + trajectory_publisher_global_->publish(trj_msg_global); + + // publish path to motion planner + path_publisher_->publish(path_msg); + path_publisher_global_->publish(path_msg_global); + + // TODO(thomas.herrmann@driveblocks.ai): outsource this to a separate method + // clear all markers in scene + visualization_msgs::msg::Marker msg_marker; + msg_marker.header = msg_mission.header; + msg_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + // this specifies the clear all / delete all action + msg_marker.action = 3; + vis_trajectory_publisher_->publish(msg_marker); + + visualization_msgs::msg::MarkerArray msg_marker_array; + msg_marker_array.markers.push_back(msg_marker); + vis_path_publisher_->publish(msg_marker_array); + + vis_trajectory_publisher_->publish(trj_vis); + vis_path_publisher_->publish(path_area); + + return; +} + +std::tuple< + autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> +MissionLaneConverterNode::ConvertMissionToTrajectory( + const mission_planner_messages::msg::MissionLanesStamped & msg) +{ + // empty trajectory for controller + autoware_auto_planning_msgs::msg::Trajectory trj_msg = + autoware_auto_planning_msgs::msg::Trajectory(); + + autoware_auto_planning_msgs::msg::Path path_msg = autoware_auto_planning_msgs::msg::Path(); + + // empty trajectory visualization message + visualization_msgs::msg::Marker trj_vis, path_center_vis, path_left_vis, path_right_vis; + visualization_msgs::msg::MarkerArray path_area_vis; + + trj_vis.header.frame_id = msg.header.frame_id; + trj_vis.header.stamp = msg.header.stamp; + trj_vis.ns = "mission_trajectory"; + trj_vis.type = visualization_msgs::msg::Marker::LINE_STRIP; + trj_vis.pose.orientation.w = 1.0; // Neutral orientation + trj_vis.scale.x = 0.4; + trj_vis.color.g = 0.742; // green color + trj_vis.color.b = 0.703; // blue color + trj_vis.color.a = 0.750; + trj_vis.lifetime.sec = 0; // forever + trj_vis.lifetime.nanosec = 0; // forever + trj_vis.frame_locked = false; // always transform into baselink + + path_left_vis.header.frame_id = msg.header.frame_id; + path_left_vis.header.stamp = msg.header.stamp; + path_left_vis.ns = "mission_path"; + path_left_vis.type = visualization_msgs::msg::Marker::LINE_STRIP; + path_left_vis.pose.orientation.w = 1.0; // Neutral orientation + path_left_vis.scale.x = 0.6; + + path_left_vis.color.g = 0.742; // green color + path_left_vis.color.b = 0.703; // blue color + path_left_vis.color.a = 0.350; + path_left_vis.lifetime.sec = 0; // forever + path_left_vis.lifetime.nanosec = 0; // forever + path_left_vis.frame_locked = false; // always transform into baselink + + path_right_vis = path_left_vis; + path_center_vis = path_left_vis; + + // fill output trajectory + // header + trj_msg.header = msg.header; + path_msg.header = msg.header; + // frame id must be "map" for Autoware controller + trj_msg.header.frame_id = "map"; + path_msg.header.frame_id = "map"; + + switch (msg.target_lane) { + case 0: + // if target == 0, forward ego lane + CreateMotionPlannerInput_( + trj_msg, path_msg, trj_vis, path_center_vis, msg.ego_lane.centerline); + + // fill path bounds left and right + CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); + CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); + break; + case -1: + // Lane change to the left + CreateMotionPlannerInput_( + trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_left[0].centerline); + + // fill path bounds left and right + CreatePathBound_( + path_msg.left_bound, path_left_vis, msg.drivable_lanes_left[0].bound_left, 1); + CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); + break; + case 1: + + // Lane change to the right + CreateMotionPlannerInput_( + trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right[0].centerline); + + // fill path bounds left and right + CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.drivable_lanes_right[0].bound_right, 2); + break; + case -2: + + // take exit left + CreateMotionPlannerInput_( + trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_left.back().centerline); + + // fill path bounds left and right + CreatePathBound_( + path_msg.left_bound, path_left_vis, msg.drivable_lanes_left.back().bound_left, 1); + CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); + break; + case 2: + + // take exit right + CreateMotionPlannerInput_( + trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right.back().centerline); + + // fill path bounds left and right + CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.drivable_lanes_right.back().bound_right, 2); + break; + + default: + RCLCPP_WARN( + this->get_logger(), "Unknown target lane specified in input mission lanes! (%d)", + msg.target_lane); + break; + } + + path_area_vis.markers.push_back(path_center_vis); + path_area_vis.markers.push_back(path_left_vis); + path_area_vis.markers.push_back(path_right_vis); + + this->AddHeadingToTrajectory_(trj_msg); + + return std::make_tuple(trj_msg, trj_vis, path_msg, path_area_vis); +} + +void MissionLaneConverterNode::CreateMotionPlannerInput_( + autoware_auto_planning_msgs::msg::Trajectory & trj_msg, + autoware_auto_planning_msgs::msg::Path & path_msg, visualization_msgs::msg::Marker & trj_vis, + visualization_msgs::msg::Marker & path_vis, + const std::vector & centerline_mission_lane) +{ + // add a mission lane's centerline to the output trajectory and path messages + for (size_t idx_point = 0; idx_point < centerline_mission_lane.size(); idx_point++) { + // check if trajectory message is empty + if (trj_msg.points.size() > 0) { + // check if last trajectory point equals the one we want to add now + if ( + trj_msg.points.back().pose.position.x != centerline_mission_lane[idx_point].x && + trj_msg.points.back().pose.position.y != centerline_mission_lane[idx_point].y) { + AddTrajectoryPoint_( + trj_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, + target_speed_); + + // similar point has to be added to the path message + AddPathPoint_( + path_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, + target_speed_); + + // add visualization marker to trajectory vis message + AddPointVisualizationMarker_( + trj_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); + + // add visualization marker to path vis message + AddPointVisualizationMarker_( + path_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); + } + } else { // add first point + // add a trajectory point + AddTrajectoryPoint_( + trj_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, + target_speed_); + + // similar point has to be added to the path message + AddPathPoint_( + path_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, + target_speed_); + + AddPointVisualizationMarker_( + trj_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); + + // add visualization marker to path vis message + AddPointVisualizationMarker_( + path_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); + } + } + + return; +} + +void MissionLaneConverterNode::CreatePathBound_( + std::vector & bound_path, visualization_msgs::msg::Marker & path_vis, + const std::vector & bound_mission_lane, const int id_marker) +{ + for (size_t idx_point = 0; idx_point < bound_mission_lane.size(); idx_point++) { + geometry_msgs::msg::Point pt_path; + pt_path.x = bound_mission_lane[idx_point].x; + pt_path.y = bound_mission_lane[idx_point].y; + // check if path bound is empty + if (bound_path.size() > 0) { + // check if last path point equals the one we want to add now + if ( + bound_path.back().x != bound_mission_lane[idx_point].x && + bound_path.back().y != bound_mission_lane[idx_point].y) { + // finally add the path point after successful checks + bound_path.push_back(pt_path); + } + } else { // add first point to path + bound_path.push_back(pt_path); + } + + // add last added point to a marker message for debugging + // FIXME: probably no unique ids for multiple calls? + AddPointVisualizationMarker_(path_vis, bound_path.back().x, bound_path.back().y, id_marker); + } + + return; +} + +void MissionLaneConverterNode::AddPathPoint_( + autoware_auto_planning_msgs::msg::Path & pth_msg, const double x, const double y, + const double v_x) +{ + // add a trajectory point + pth_msg.points.push_back(autoware_auto_planning_msgs::msg::PathPoint()); + + // fill trajectory points with meaningful data + pth_msg.points.back().pose.position.x = x; + pth_msg.points.back().pose.position.y = y; + + // constant velocity + pth_msg.points.back().longitudinal_velocity_mps = v_x; + + return; +} + +void MissionLaneConverterNode::AddTrajectoryPoint_( + autoware_auto_planning_msgs::msg::Trajectory & trj_msg, const double x, const double y, + const double v_x) +{ + // add a trajectory point + trj_msg.points.push_back(autoware_auto_planning_msgs::msg::TrajectoryPoint()); + + // fill trajectory points with meaningful data + trj_msg.points.back().pose.position.x = x; + trj_msg.points.back().pose.position.y = y; + + // constant velocity + trj_msg.points.back().longitudinal_velocity_mps = v_x; + + return; +} + +void MissionLaneConverterNode::AddPointVisualizationMarker_( + visualization_msgs::msg::Marker & trj_vis, const double x, const double y, const int id_marker) +{ + // fill visualization message + trj_vis.points.push_back(geometry_msgs::msg::Point()); + + trj_vis.points.back().x = x; + trj_vis.points.back().y = y; + trj_vis.id = id_marker; + + return; +} + +void MissionLaneConverterNode::AddHeadingToTrajectory_( + autoware_auto_planning_msgs::msg::Trajectory & trj_msg) +{ + std::vector points; + + for (size_t idx_point = 0; idx_point < trj_msg.points.size(); idx_point++) { + points.push_back(geometry_msgs::msg::Point()); + points.back().x = trj_msg.points[idx_point].pose.position.x; + points.back().y = trj_msg.points[idx_point].pose.position.y; + } + + // only execute if we have at least 2 points + if (points.size() > 1) { + std::vector psi_vec = GetPsiForPoints(points); + + tf2::Quaternion tf2_quat; + for (size_t idx_point = 0; idx_point < trj_msg.points.size(); idx_point++) { + tf2_quat.setRPY(0.0, 0.0, psi_vec[idx_point]); + + trj_msg.points.back().pose.orientation.x = tf2_quat.getX(); + trj_msg.points.back().pose.orientation.y = tf2_quat.getY(); + trj_msg.points.back().pose.orientation.z = tf2_quat.getZ(); + trj_msg.points.back().pose.orientation.w = tf2_quat.getW(); + } + } + + return; +} + +// TODO: store the latest odometry message here and then re-use in the output conversion +void MissionLaneConverterNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg) +{ + // store current odometry information + last_odom_msg_ = msg; + + if (!received_motion_update_once_) { + initial_odom_msg_ = msg; + } + + received_motion_update_once_ = true; + + visualization_msgs::msg::Marker odom_vis; + odom_vis.header.frame_id = msg.header.frame_id; + odom_vis.header.stamp = msg.header.stamp; + odom_vis.ns = "odometry"; + odom_vis.type = visualization_msgs::msg::Marker::POINTS; + odom_vis.pose.orientation.w = 1.0; // Neutral orientation + odom_vis.scale.x = 5.0; + odom_vis.color.g = 0.0; // green color + odom_vis.color.b = 0.7; // blue color + odom_vis.color.a = 0.7; + odom_vis.lifetime.sec = 0; // forever + odom_vis.lifetime.nanosec = 0; // forever + odom_vis.frame_locked = false; // always transform into baselink + + odom_vis.points.push_back(geometry_msgs::msg::Point()); + + odom_vis.points.back().x = msg.pose.pose.position.x; + odom_vis.points.back().y = msg.pose.pose.position.y; + odom_vis.id = 100; + + vis_odometry_publisher_global_->publish(odom_vis); + + return; +} + +template +T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) +{ + // define output message + T msg_output = msg_input; + msg_output.header.frame_id = "map"; + + // Construct raw odometry pose + geometry_msgs::msg::PoseStamped odometry_pose_raw, pose_base_link_in_odom_frame, + pose_base_link_in_map_frame; + odometry_pose_raw.header = last_odom_msg_.header; + odometry_pose_raw.pose = last_odom_msg_.pose.pose; + + // If the incoming odometry signal is properly filled, i.e. if the frame ids + // are given and report an odometry signal , do nothing, else we assume the + // odometry signal stems from the GNSS (and is therefore valid in the odom + // frame) + if (last_odom_msg_.header.frame_id == "map" && last_odom_msg_.child_frame_id == "base_link") { + pose_base_link_in_map_frame = odometry_pose_raw; + } else { + if (!b_global_odometry_deprecation_warning_) { + RCLCPP_WARN( + this->get_logger(), + "Your odometry signal doesn't match the expectation to be a " + "transformation from frame to ! Check your odometry frames or provide a " + "proper conversion!"); + b_global_odometry_deprecation_warning_ = true; + } + } + + if (received_motion_update_once_) { + const double psi_initial = GetYawFromQuaternion( + initial_odom_msg_.pose.pose.orientation.x, initial_odom_msg_.pose.pose.orientation.y, + initial_odom_msg_.pose.pose.orientation.z, initial_odom_msg_.pose.pose.orientation.w); + const Pose2D pose_initial( + initial_odom_msg_.pose.pose.position.x, initial_odom_msg_.pose.pose.position.y, psi_initial); + + const double psi_cur = GetYawFromQuaternion( + last_odom_msg_.pose.pose.orientation.x, last_odom_msg_.pose.pose.orientation.y, + last_odom_msg_.pose.pose.orientation.z, last_odom_msg_.pose.pose.orientation.w); + const Pose2D pose_cur( + last_odom_msg_.pose.pose.position.x, last_odom_msg_.pose.pose.position.y, psi_cur); + + // get relationship from current odom frame to global map frame origin + const Pose2D d_current_to_map_origin = TransformToNewCosy2D(pose_cur, Pose2D{0.0, 0.0}); + + // convert all the input points to the global map frame + for (size_t i = 0; i < msg_input.points.size(); i++) { + // convert input pose + const double psi_point = GetYawFromQuaternion( + msg_input.points[i].pose.orientation.x, msg_input.points[i].pose.orientation.y, + msg_input.points[i].pose.orientation.z, msg_input.points[i].pose.orientation.w); + const Pose2D pose( + msg_input.points[i].pose.position.x, msg_input.points[i].pose.position.y, psi_point); + + // express point in global map frame + Pose2D pose_map = TransformToNewCosy2D(d_current_to_map_origin, pose); + + msg_output.points[i].pose.position.x = pose_map.get_x(); + msg_output.points[i].pose.position.y = pose_map.get_y(); + } + + // convert the path area's bounds + if constexpr (std::is_same::value) { + for (size_t ib = 0; ib < 2; ib++) { + std::vector bound; + if (ib == 0) + bound = msg_input.left_bound; + else + bound = msg_input.right_bound; + for (size_t i = 0; i < bound.size(); i++) { + // convert input pose + const Pose2D pose(bound[i].x, bound[i].y); + + // express point in global map frame + Pose2D pose_map = TransformToNewCosy2D(d_current_to_map_origin, pose); + + if (ib == 0) { + msg_output.left_bound[i].x = pose_map.get_x(); + msg_output.left_bound[i].y = pose_map.get_y(); + } else { + msg_output.right_bound[i].x = pose_map.get_x(); + msg_output.right_bound[i].y = pose_map.get_y(); + } + } + } + } + + // add heading if type is a trajectory + if constexpr (std::is_same::value) + AddHeadingToTrajectory_(msg_output); + } + + return msg_output; +} + +visualization_msgs::msg::Marker MissionLaneConverterNode::GetGlobalTrjVisualization_( + const autoware_auto_planning_msgs::msg::Trajectory & trj_msg) +{ + // empty trajectory visualization message + visualization_msgs::msg::Marker trj_vis_global; + trj_vis_global.header.frame_id = trj_msg.header.frame_id; + trj_vis_global.header.stamp = trj_msg.header.stamp; + trj_vis_global.ns = "mission_trajectory_global"; + trj_vis_global.type = visualization_msgs::msg::Marker::LINE_STRIP; + trj_vis_global.pose.orientation.w = 1.0; // Neutral orientation + trj_vis_global.scale.x = 0.4; + trj_vis_global.color.g = 0.0; // green color + trj_vis_global.color.b = 0.703; // blue color + trj_vis_global.color.a = 0.0; + trj_vis_global.lifetime.sec = 0; // forever + trj_vis_global.lifetime.nanosec = 0; // forever + trj_vis_global.frame_locked = false; // always transform into baselink + + for (size_t i = 0; i < trj_msg.points.size(); i++) { + AddPointVisualizationMarker_( + trj_vis_global, trj_msg.points[i].pose.position.x, trj_msg.points[i].pose.position.y, 10); + } + + return trj_vis_global; +} diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/test/gtest_main.cpp b/planning/mapless_architecture/src/autoware_mission_lane_converter/test/gtest_main.cpp new file mode 100644 index 0000000000000..f00c8d543b7dc --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/test/gtest_main.cpp @@ -0,0 +1,18 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "gtest/gtest.h" +#include "include/test_mission_planner_converter.hpp" +#include "rclcpp/rclcpp.hpp" + +TEST(MissionConverter, MissionToTrajectory) +{ + EXPECT_EQ(TestMissionToTrajectory(), 0); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); + rclcpp::shutdown(); +} diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp b/planning/mapless_architecture/src/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp new file mode 100644 index 0000000000000..ae2ddd52ff89f --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp @@ -0,0 +1,9 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#ifndef TEST_MISSION_PLANNER_CONVERTER_HPP_ +#define TEST_MISSION_PLANNER_CONVERTER_HPP_ + +int TestMissionToTrajectory(); + +#endif // TEST_MISSION_PLANNER_CONVERTER_HPP_ diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/src/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp new file mode 100644 index 0000000000000..b0e4f2bde1825 --- /dev/null +++ b/planning/mapless_architecture/src/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -0,0 +1,113 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license + +#include "gtest/gtest.h" +#include "mission_lane_converter_node.hpp" +#include "mission_planner_messages/msg/driving_corridor.hpp" +#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" + +#include "geometry_msgs/msg/point.hpp" + +int TestMissionToTrajectory() +{ + MissionLaneConverterNode mission_converter = MissionLaneConverterNode(); + + mission_planner_messages::msg::MissionLanesStamped mission_msg; + + // set target lane to ego lane + mission_msg.target_lane = 0; + + // add a driving corridor to the ego lane + mission_msg.ego_lane = mission_planner_messages::msg::DrivingCorridor(); + + // add points to the ego lane centerline + mission_msg.ego_lane.centerline.push_back(geometry_msgs::msg::Point()); + mission_msg.ego_lane.centerline.back().x = 0.0; + mission_msg.ego_lane.centerline.back().y = 0.0; + + // get converted trajectory + std::tuple< + autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); + + // extract trajectory + autoware_auto_planning_msgs::msg::Trajectory trj_msg = std::get<0>(mission_to_trj); + + EXPECT_EQ(trj_msg.points.back().pose.position.x, mission_msg.ego_lane.centerline.back().x); + EXPECT_EQ(trj_msg.points.back().pose.position.y, mission_msg.ego_lane.centerline.back().y); + EXPECT_EQ(trj_msg.points.back().pose.orientation.x, 0.0); + EXPECT_EQ(trj_msg.points.back().pose.orientation.y, 0.0); + EXPECT_EQ(trj_msg.points.back().pose.orientation.z, 0.0); + EXPECT_EQ(trj_msg.points.back().pose.orientation.w, 1.0); + + // TEST 2: some more points on the ego lane + // add points to the ego lane centerline + mission_msg.ego_lane.centerline.push_back(geometry_msgs::msg::Point()); + mission_msg.ego_lane.centerline.back().x = 1.0; + mission_msg.ego_lane.centerline.back().y = 1.0; + mission_msg.ego_lane.centerline.push_back(geometry_msgs::msg::Point()); + mission_msg.ego_lane.centerline.back().x = 2.0; + mission_msg.ego_lane.centerline.back().y = 2.0; + + // convert + mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); + + // extract trajectory + trj_msg = std::get<0>(mission_to_trj); + + EXPECT_EQ(trj_msg.points.back().pose.position.x, mission_msg.ego_lane.centerline.back().x); + EXPECT_EQ(trj_msg.points.back().pose.position.y, mission_msg.ego_lane.centerline.back().y); + + // TEST 3: neighbor lane left + mission_msg.drivable_lanes_left.push_back(mission_planner_messages::msg::DrivingCorridor()); + mission_msg.drivable_lanes_left.back().centerline.push_back(geometry_msgs::msg::Point()); + mission_msg.drivable_lanes_left.back().centerline.back().x = 0.0; + mission_msg.drivable_lanes_left.back().centerline.back().y = 0.0; + mission_msg.drivable_lanes_left.back().centerline.push_back(geometry_msgs::msg::Point()); + mission_msg.drivable_lanes_left.back().centerline.back().x = 5.0; + mission_msg.drivable_lanes_left.back().centerline.back().y = 3.0; + + // set target lane to neighbor left + mission_msg.target_lane = -1; + + // convert + mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); + + // extract trajectory + trj_msg = std::get<0>(mission_to_trj); + + EXPECT_EQ( + trj_msg.points.back().pose.position.x, + mission_msg.drivable_lanes_left.back().centerline.back().x); + EXPECT_EQ( + trj_msg.points.back().pose.position.y, + mission_msg.drivable_lanes_left.back().centerline.back().y); + + // TEST 4: neighbor lane right + mission_msg.drivable_lanes_right.push_back(mission_planner_messages::msg::DrivingCorridor()); + mission_msg.drivable_lanes_right.back().centerline.push_back(geometry_msgs::msg::Point()); + mission_msg.drivable_lanes_right.back().centerline.back().x = 1.0; + mission_msg.drivable_lanes_right.back().centerline.back().y = 1.2; + mission_msg.drivable_lanes_right.back().centerline.push_back(geometry_msgs::msg::Point()); + mission_msg.drivable_lanes_right.back().centerline.back().x = 3.0; + mission_msg.drivable_lanes_right.back().centerline.back().y = 3.6; + + // set target lane to neighbor right + mission_msg.target_lane = 1; + + // convert + mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); + + // extract trajectory + trj_msg = std::get<0>(mission_to_trj); + + EXPECT_EQ( + trj_msg.points.back().pose.position.x, + mission_msg.drivable_lanes_right.back().centerline.back().x); + EXPECT_EQ( + trj_msg.points.back().pose.position.y, + mission_msg.drivable_lanes_right.back().centerline.back().y); + + return 0; +} diff --git a/planning/mapless_architecture/src/local_mission_planner_common/CMakeLists.txt b/planning/mapless_architecture/src/local_mission_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..67a2439354f55 --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.8) +project(lib_mission_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(autoware_cmake REQUIRED) # automatically find dependencies +ament_auto_find_build_dependencies() +autoware_package() + +# add library to be exported +add_library(${PROJECT_NAME} SHARED + src/helper_functions.cpp) + +include_directories(include) + +# add dependent libraries +ament_target_dependencies(${PROJECT_NAME} + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + mission_planner_messages + db_msgs) + +# include public headers +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +# export library +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +# export dependent libraries +ament_export_dependencies( + geometry_msgs + tf2 + tf2_ros + tf2_geometry_msgs + mission_planner_messages + db_msgs) + +# install library +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# configure clang format ----------------------------------------------------------------------------------------------- +set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) + +# testing -------------------------------------------------------------------------------------------------------------- +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + + ament_auto_add_gtest(${PROJECT_NAME}_tests + # MAIN + test/gtest_main.cpp + # + test/src/test_helper_functions.cpp src/helper_functions.cpp) + + ament_lint_auto_find_test_dependencies() + target_include_directories(${PROJECT_NAME}_tests PRIVATE test/include) +endif() + +ament_package() diff --git a/planning/mapless_architecture/src/local_mission_planner_common/Readme.md b/planning/mapless_architecture/src/local_mission_planner_common/Readme.md new file mode 100644 index 0000000000000..898c81d39e7d3 --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/Readme.md @@ -0,0 +1,3 @@ +# lib_mission_planner + +The lib_mission_planner contains shared code utilized by various nodes. The code includes geometry helper functions, a Pose2D class, and coordinate transformations. diff --git a/planning/mapless_architecture/src/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp b/planning/mapless_architecture/src/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp new file mode 100644 index 0000000000000..e2c542ba79c51 --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp @@ -0,0 +1,104 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ +#define LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" +#include "mission_planner_messages/msg/road_segments.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "db_msgs/msg/lanelets_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" + +#include + +namespace lib_mission_planner +{ + +/** + * @brief A class for a 2D pose. + * + */ +class Pose2D +{ +public: + Pose2D(); + Pose2D(const double x, const double y, const double psi = 0.0); + + // accessors and mutators + double get_x() const; + double get_y() const; + Eigen::Vector2d get_xy() const; + double get_psi() const; + geometry_msgs::msg::Point get_point() const; + void set_x(const double x); + void set_y(const double y); + void set_xy(const double x, const double y); + void set_xy(const Eigen::Vector2d xy); + void set_psi(const double psi); + +private: + // data variables + Eigen::Vector2d xy_; + double psi_; +}; + +/** + * Represent a 2D pose (pose_prev) in a new origin / coordinate system, which + * is given in relation to the previous coordinate system / origin + * (cosy_rel). + * + * If this relation is not known, it can be calculated with this function by + * providing the absolute pose of the new cosy as "pose_prev" and the + * absolute pose of the old/current cosy as "cosy_rel". + * + * @param cosy_rel translation and rotation between the current/old cosy and + * a new/go-to cosy + * @param pose_prev coordinates and heading of a pose in the current/old cosy + * @return Pose coordinates and heading of pose_prev in the new cosy + * (defined by the shift cosy_rel between previous and current cosy) + */ +Pose2D TransformToNewCosy2D(const Pose2D cosy_rel, const Pose2D pose_prev); + +/** + * @brief Get the yaw value from a quaternion. + * + * @param x The x value. + * @param y The y value. + * @param z The z value. + * @param w The w value. + * @return double + */ +double GetYawFromQuaternion(const double x, const double y, const double z, const double w); + +/** + * @brief Normalize the psi value. + * + * @param psi The psi value. + * @return double + */ +double NormalizePsi(const double psi); + +/** + * @brief Get the psi value given some points. + * + * @param points The points (std::vector + */ +std::vector GetPsiForPoints(const std::vector & points); + +/** + * @brief Convert the LaneletsStamped message into a RoadSegments message. + * + * @param msg The message (db_msgs::msg::LaneletsStamped). + * @return mission_planner_messages::msg::RoadSegments. + */ +mission_planner_messages::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( + const db_msgs::msg::LaneletsStamped & msg); + +} // namespace lib_mission_planner + +#endif // LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/src/local_mission_planner_common/package.xml b/planning/mapless_architecture/src/local_mission_planner_common/package.xml new file mode 100644 index 0000000000000..593f2d803a001 --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/package.xml @@ -0,0 +1,25 @@ + + + + lib_mission_planner + 0.0.0 + Library containing helper functions for the different nodes. + driveblocks + driveblocks proprietary license + + autoware_cmake + + db_msgs + geometry_msgs + mission_planner_messages + tf2 + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mapless_architecture/src/local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/src/local_mission_planner_common/src/helper_functions.cpp new file mode 100644 index 0000000000000..4926662c5125f --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/src/helper_functions.cpp @@ -0,0 +1,179 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "lib_mission_planner/helper_functions.hpp" + +#include "mission_planner_messages/msg/road_segments.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "db_msgs/msg/lanelets_stamped.hpp" + +namespace lib_mission_planner +{ + +double NormalizePsi(const double psi) +{ + // remove multiples of 2 PI by using modulo on absolute value and apply sign + double psi_out = copysign(fmod(fabs(psi), M_PI * 2.0), psi); + + // restrict psi to [-pi, pi[ + if (psi_out >= M_PI) + psi_out -= 2.0 * M_PI; + else if (psi_out < -M_PI) + psi_out += 2.0 * M_PI; + + return psi_out; +} + +std::vector GetPsiForPoints(const std::vector & points) +{ + int num_points = points.size(); + std::vector tang_vecs(num_points * 2); + + // get heading vector for first point + tang_vecs[0] = points[1].x - points[0].x; + tang_vecs[1] = points[1].y - points[0].y; + + // use one point before and after the targeted one for heading vector (more + // stable/robust than relying on a single pair of points) + for (int i = 1; i < num_points - 1; i++) { + tang_vecs[2 * i] = points[i + 1].x - points[i - 1].x; + tang_vecs[2 * i + 1] = points[i + 1].y - points[i - 1].y; + } + + // get heading vector for last point + tang_vecs[2 * (num_points - 1)] = points[num_points - 1].x - points[num_points - 2].x; + tang_vecs[2 * (num_points - 1) + 1] = points[num_points - 1].y - points[num_points - 2].y; + + // calculate heading angles with atan2 + std::vector psi_points(num_points); + for (int i = 0; i < num_points; i++) { + psi_points[i] = NormalizePsi(std::atan2(tang_vecs[2 * i + 1], tang_vecs[2 * i])); + } + + return psi_points; +} + +Pose2D::Pose2D() +{ + this->set_xy(0.0, 0.0); + this->set_psi(0.0); +} +Pose2D::Pose2D(const double x, const double y, const double psi /* = 0.0 */) +{ + this->set_xy(x, y); + this->set_psi(psi); +} +double Pose2D::get_x() const +{ + return this->xy_(0); +} +double Pose2D::get_y() const +{ + return this->xy_(1); +} +Eigen::Vector2d Pose2D::get_xy() const +{ + return this->xy_; +} +double Pose2D::get_psi() const +{ + return this->psi_; +} +geometry_msgs::msg::Point Pose2D::get_point() const +{ + geometry_msgs::msg::Point tmp_point; + tmp_point.x = this->xy_(0); + tmp_point.y = this->xy_(1); + return tmp_point; +} +void Pose2D::set_x(const double x) +{ + this->xy_(0) = x; +} +void Pose2D::set_y(const double y) +{ + this->xy_(1) = y; +} +void Pose2D::set_xy(const double x, const double y) +{ + this->xy_ = {x, y}; +} +void Pose2D::set_xy(const Eigen::Vector2d xy) +{ + this->xy_ = xy; +} +void Pose2D::set_psi(const double psi) +{ + this->psi_ = psi; +} + +Pose2D TransformToNewCosy2D(const Pose2D cosy_rel, const Pose2D pose_prev) +{ + Pose2D pose_out; + + pose_out.set_xy( + Eigen::Rotation2D(-cosy_rel.get_psi()) * Eigen::Translation2d(-cosy_rel.get_xy()) * + pose_prev.get_xy()); + pose_out.set_psi(NormalizePsi(pose_prev.get_psi() - cosy_rel.get_psi())); + + return pose_out; +} + +double GetYawFromQuaternion(const double x, const double y, const double z, const double w) +{ + tf2::Quaternion tmp_quat(x, y, z, w); + tf2::Matrix3x3 m(tmp_quat); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} + +// Declare a static logger +static rclcpp::Logger static_logger = rclcpp::get_logger("static_logger"); + +mission_planner_messages::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( + const db_msgs::msg::LaneletsStamped & msg) +{ + // Initialize road segments message + mission_planner_messages::msg::RoadSegments road_segments; + + // Fill message header and pose + road_segments.header = msg.header; + road_segments.pose = msg.pose; + + // Convert lanelets to segments if lanelets are not empty + if (!msg.lanelets.empty()) { + for (const db_msgs::msg::Lanelet & lanelet : msg.lanelets) { + // Initialize a segment + mission_planner_messages::msg::Segment segment; + + // Fill the segment with basic information + segment.id = lanelet.id; + segment.successor_lanelet_id = lanelet.successor_lanelet_id; + segment.neighboring_lanelet_id = lanelet.neighboring_lanelet_id; + + // Copy linestrings data + for (int i = 0; i < 2; ++i) { + // Copy points from the original linestring to the new one if points are not empty + if (!lanelet.linestrings[i].points.empty()) { + segment.linestrings[i].poses.reserve(lanelet.linestrings[i].points.size()); + + for (const db_msgs::msg::DBPoint & point : lanelet.linestrings[i].points) { + segment.linestrings[i].poses.push_back(point.pose); + } + } else { + RCLCPP_WARN( + static_logger, + "Linestring does not contain points (ConvertLaneletsStamped2RoadSegments)!"); + } + } + + // Add the filled segment to the road_segments message + road_segments.segments.push_back(segment); + } + } + + return road_segments; +} + +} // namespace lib_mission_planner diff --git a/planning/mapless_architecture/src/local_mission_planner_common/test/gtest_main.cpp b/planning/mapless_architecture/src/local_mission_planner_common/test/gtest_main.cpp new file mode 100644 index 0000000000000..27aada7ff9823 --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/test/gtest_main.cpp @@ -0,0 +1,14 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "gtest/gtest.h" +#include "test_helper_functions.hpp" + +/** + * Test helper functions + */ + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/mapless_architecture/src/local_mission_planner_common/test/include/test_helper_functions.hpp b/planning/mapless_architecture/src/local_mission_planner_common/test/include/test_helper_functions.hpp new file mode 100644 index 0000000000000..6ad8f869ae752 --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/test/include/test_helper_functions.hpp @@ -0,0 +1,10 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#ifndef TEST_HELPER_FUNCTIONS_HPP_ +#define TEST_HELPER_FUNCTIONS_HPP_ + +#include "lib_mission_planner/helper_functions.hpp" + +// TODO(simon.eisenmann@driveblocks.ai): Add test functions + +#endif // TEST_HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/src/local_mission_planner_common/test/src/test_helper_functions.cpp b/planning/mapless_architecture/src/local_mission_planner_common/test/src/test_helper_functions.cpp new file mode 100644 index 0000000000000..bcbd5c7faf704 --- /dev/null +++ b/planning/mapless_architecture/src/local_mission_planner_common/test/src/test_helper_functions.cpp @@ -0,0 +1,8 @@ +// Copyright 2024 driveblocks GmbH +// driveblocks proprietary license +#include "test_helper_functions.hpp" + +#include "gtest/gtest.h" +#include "lib_mission_planner/helper_functions.hpp" + +// TODO(simon.eisenmann@driveblocks.ai): Add test functions From 0b6c3066d5548172cd05afd5eb67001f8a4e3e80 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 13 Jun 2024 12:45:15 +0200 Subject: [PATCH 02/24] Restructure folder Signed-off-by: Simon Eisenmann --- .../mapless_architecture/{src => }/autoware_hmi/CMakeLists.txt | 0 planning/mapless_architecture/{src => }/autoware_hmi/Readme.md | 0 .../{src => }/autoware_hmi/include/hmi_node.hpp | 0 .../{src => }/autoware_hmi/launch/hmi.launch.py | 0 planning/mapless_architecture/{src => }/autoware_hmi/package.xml | 0 .../mapless_architecture/{src => }/autoware_hmi/src/hmi_main.cpp | 0 .../mapless_architecture/{src => }/autoware_hmi/src/hmi_node.cpp | 0 .../{src => }/autoware_hmi/test/gtest_main.cpp | 0 .../{src => }/autoware_local_map_provider/CMakeLists.txt | 0 .../{src => }/autoware_local_map_provider/Readme.md | 0 .../include/local_map_provider_node.hpp | 0 .../launch/local_map_provider.launch.py | 0 .../{src => }/autoware_local_map_provider/package.xml | 0 .../autoware_local_map_provider/src/local_map_provider_main.cpp | 0 .../autoware_local_map_provider/src/local_map_provider_node.cpp | 0 .../{src => }/autoware_local_map_provider/test/gtest_main.cpp | 0 .../{src => }/autoware_local_mission_planner/CMakeLists.txt | 0 .../{src => }/autoware_local_mission_planner/Readme.md | 0 .../include/mission_planner_node.hpp | 0 .../launch/mission_planner.launch.py | 0 .../launch/mission_planner_compose.launch.py | 0 .../{src => }/autoware_local_mission_planner/package.xml | 0 .../param/mission_planner_default.yaml | 0 .../autoware_local_mission_planner/src/mission_planner_main.cpp | 0 .../autoware_local_mission_planner/src/mission_planner_node.cpp | 0 .../{src => }/autoware_local_mission_planner/test/gtest_main.cpp | 0 .../test/include/test_mission_planner_core.hpp | 0 .../test/src/test_mission_planner_core.cpp | 0 .../{src => }/autoware_local_road_provider/CMakeLists.txt | 0 .../{src => }/autoware_local_road_provider/Readme.md | 0 .../include/local_road_provider_node.hpp | 0 .../launch/local_road_provider.launch.py | 0 .../{src => }/autoware_local_road_provider/package.xml | 0 .../autoware_local_road_provider/src/local_road_provider_main.cpp | 0 .../autoware_local_road_provider/src/local_road_provider_node.cpp | 0 .../{src => }/autoware_local_road_provider/test/gtest_main.cpp | 0 .../{src => }/autoware_mission_lane_converter/CMakeLists.txt | 0 .../{src => }/autoware_mission_lane_converter/Readme.md | 0 .../include/mission_lane_converter_node.hpp | 0 .../launch/mission_lane_converter.launch.py | 0 .../{src => }/autoware_mission_lane_converter/package.xml | 0 .../param/mission_lane_converter_default.yaml | 0 .../src/main_mission_lane_converter.cpp | 0 .../src/mission_lane_converter_node.cpp | 0 .../{src => }/autoware_mission_lane_converter/test/gtest_main.cpp | 0 .../test/include/test_mission_planner_converter.hpp | 0 .../test/src/test_mission_planner_converter.cpp | 0 .../{src => }/local_mission_planner_common/CMakeLists.txt | 0 .../{src => }/local_mission_planner_common/Readme.md | 0 .../include/lib_mission_planner/helper_functions.hpp | 0 .../{src => }/local_mission_planner_common/package.xml | 0 .../local_mission_planner_common/src/helper_functions.cpp | 0 .../{src => }/local_mission_planner_common/test/gtest_main.cpp | 0 .../test/include/test_helper_functions.hpp | 0 .../test/src/test_helper_functions.cpp | 0 55 files changed, 0 insertions(+), 0 deletions(-) rename planning/mapless_architecture/{src => }/autoware_hmi/CMakeLists.txt (100%) rename planning/mapless_architecture/{src => }/autoware_hmi/Readme.md (100%) rename planning/mapless_architecture/{src => }/autoware_hmi/include/hmi_node.hpp (100%) rename planning/mapless_architecture/{src => }/autoware_hmi/launch/hmi.launch.py (100%) rename planning/mapless_architecture/{src => }/autoware_hmi/package.xml (100%) rename planning/mapless_architecture/{src => }/autoware_hmi/src/hmi_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_hmi/src/hmi_node.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_hmi/test/gtest_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/CMakeLists.txt (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/Readme.md (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/include/local_map_provider_node.hpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/launch/local_map_provider.launch.py (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/package.xml (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/src/local_map_provider_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/src/local_map_provider_node.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_map_provider/test/gtest_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/CMakeLists.txt (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/Readme.md (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/include/mission_planner_node.hpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/launch/mission_planner.launch.py (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/launch/mission_planner_compose.launch.py (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/package.xml (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/param/mission_planner_default.yaml (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/src/mission_planner_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/src/mission_planner_node.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/test/gtest_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/CMakeLists.txt (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/Readme.md (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/include/local_road_provider_node.hpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/launch/local_road_provider.launch.py (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/package.xml (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/src/local_road_provider_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/src/local_road_provider_node.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_local_road_provider/test/gtest_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/CMakeLists.txt (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/Readme.md (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/package.xml (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/test/gtest_main.cpp (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp (100%) rename planning/mapless_architecture/{src => }/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/CMakeLists.txt (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/Readme.md (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/package.xml (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/src/helper_functions.cpp (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/test/gtest_main.cpp (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/test/include/test_helper_functions.hpp (100%) rename planning/mapless_architecture/{src => }/local_mission_planner_common/test/src/test_helper_functions.cpp (100%) diff --git a/planning/mapless_architecture/src/autoware_hmi/CMakeLists.txt b/planning/mapless_architecture/autoware_hmi/CMakeLists.txt similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/CMakeLists.txt rename to planning/mapless_architecture/autoware_hmi/CMakeLists.txt diff --git a/planning/mapless_architecture/src/autoware_hmi/Readme.md b/planning/mapless_architecture/autoware_hmi/Readme.md similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/Readme.md rename to planning/mapless_architecture/autoware_hmi/Readme.md diff --git a/planning/mapless_architecture/src/autoware_hmi/include/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/include/hmi_node.hpp rename to planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp diff --git a/planning/mapless_architecture/src/autoware_hmi/launch/hmi.launch.py b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/launch/hmi.launch.py rename to planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py diff --git a/planning/mapless_architecture/src/autoware_hmi/package.xml b/planning/mapless_architecture/autoware_hmi/package.xml similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/package.xml rename to planning/mapless_architecture/autoware_hmi/package.xml diff --git a/planning/mapless_architecture/src/autoware_hmi/src/hmi_main.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/src/hmi_main.cpp rename to planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp diff --git a/planning/mapless_architecture/src/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/src/hmi_node.cpp rename to planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp diff --git a/planning/mapless_architecture/src/autoware_hmi/test/gtest_main.cpp b/planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_hmi/test/gtest_main.cpp rename to planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/CMakeLists.txt b/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/CMakeLists.txt rename to planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/Readme.md b/planning/mapless_architecture/autoware_local_map_provider/Readme.md similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/Readme.md rename to planning/mapless_architecture/autoware_local_map_provider/Readme.md diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/include/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/include/local_map_provider_node.hpp rename to planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/launch/local_map_provider.launch.py b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/launch/local_map_provider.launch.py rename to planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/package.xml b/planning/mapless_architecture/autoware_local_map_provider/package.xml similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/package.xml rename to planning/mapless_architecture/autoware_local_map_provider/package.xml diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_main.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_main.cpp rename to planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/src/local_map_provider_node.cpp rename to planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp diff --git a/planning/mapless_architecture/src/autoware_local_map_provider/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_map_provider/test/gtest_main.cpp rename to planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/CMakeLists.txt rename to planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/Readme.md b/planning/mapless_architecture/autoware_local_mission_planner/Readme.md similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/Readme.md rename to planning/mapless_architecture/autoware_local_mission_planner/Readme.md diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/include/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/include/mission_planner_node.hpp rename to planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner.launch.py rename to planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner_compose.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/launch/mission_planner_compose.launch.py rename to planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/autoware_local_mission_planner/package.xml similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/package.xml rename to planning/mapless_architecture/autoware_local_mission_planner/package.xml diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/param/mission_planner_default.yaml b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/param/mission_planner_default.yaml rename to planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_main.cpp rename to planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/src/mission_planner_node.cpp rename to planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/test/gtest_main.cpp rename to planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp rename to planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp diff --git a/planning/mapless_architecture/src/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp rename to planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/CMakeLists.txt b/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/CMakeLists.txt rename to planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/Readme.md b/planning/mapless_architecture/autoware_local_road_provider/Readme.md similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/Readme.md rename to planning/mapless_architecture/autoware_local_road_provider/Readme.md diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/include/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/include/local_road_provider_node.hpp rename to planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/launch/local_road_provider.launch.py b/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/launch/local_road_provider.launch.py rename to planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/package.xml b/planning/mapless_architecture/autoware_local_road_provider/package.xml similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/package.xml rename to planning/mapless_architecture/autoware_local_road_provider/package.xml diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_main.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_main.cpp rename to planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/src/local_road_provider_node.cpp rename to planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp diff --git a/planning/mapless_architecture/src/autoware_local_road_provider/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_road_provider/test/gtest_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_local_road_provider/test/gtest_main.cpp rename to planning/mapless_architecture/autoware_local_road_provider/test/gtest_main.cpp diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/CMakeLists.txt b/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/CMakeLists.txt rename to planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/Readme.md b/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/Readme.md rename to planning/mapless_architecture/autoware_mission_lane_converter/Readme.md diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp rename to planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py rename to planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/package.xml b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/package.xml rename to planning/mapless_architecture/autoware_mission_lane_converter/package.xml diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml b/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml rename to planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp rename to planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp rename to planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/test/gtest_main.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/test/gtest_main.cpp rename to planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp rename to planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp diff --git a/planning/mapless_architecture/src/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp similarity index 100% rename from planning/mapless_architecture/src/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp rename to planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp diff --git a/planning/mapless_architecture/src/local_mission_planner_common/CMakeLists.txt b/planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/CMakeLists.txt rename to planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt diff --git a/planning/mapless_architecture/src/local_mission_planner_common/Readme.md b/planning/mapless_architecture/local_mission_planner_common/Readme.md similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/Readme.md rename to planning/mapless_architecture/local_mission_planner_common/Readme.md diff --git a/planning/mapless_architecture/src/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp b/planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp rename to planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp diff --git a/planning/mapless_architecture/src/local_mission_planner_common/package.xml b/planning/mapless_architecture/local_mission_planner_common/package.xml similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/package.xml rename to planning/mapless_architecture/local_mission_planner_common/package.xml diff --git a/planning/mapless_architecture/src/local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/src/helper_functions.cpp rename to planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp diff --git a/planning/mapless_architecture/src/local_mission_planner_common/test/gtest_main.cpp b/planning/mapless_architecture/local_mission_planner_common/test/gtest_main.cpp similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/test/gtest_main.cpp rename to planning/mapless_architecture/local_mission_planner_common/test/gtest_main.cpp diff --git a/planning/mapless_architecture/src/local_mission_planner_common/test/include/test_helper_functions.hpp b/planning/mapless_architecture/local_mission_planner_common/test/include/test_helper_functions.hpp similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/test/include/test_helper_functions.hpp rename to planning/mapless_architecture/local_mission_planner_common/test/include/test_helper_functions.hpp diff --git a/planning/mapless_architecture/src/local_mission_planner_common/test/src/test_helper_functions.cpp b/planning/mapless_architecture/local_mission_planner_common/test/src/test_helper_functions.cpp similarity index 100% rename from planning/mapless_architecture/src/local_mission_planner_common/test/src/test_helper_functions.cpp rename to planning/mapless_architecture/local_mission_planner_common/test/src/test_helper_functions.cpp From 8b9c50481ac892756fb6509bcb4e32af9c5c0c70 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 13 Jun 2024 13:44:57 +0200 Subject: [PATCH 03/24] Change documentation Signed-off-by: Simon Eisenmann --- planning/mapless_architecture/README.md | 4 +- .../autoware_local_map_provider/Readme.md | 2 +- .../autoware_local_road_provider/Readme.md | 2 +- .../images/mapless_architecture.svg | 776 ++++++++++++++++++ 4 files changed, 781 insertions(+), 3 deletions(-) create mode 100644 planning/mapless_architecture/images/mapless_architecture.svg diff --git a/planning/mapless_architecture/README.md b/planning/mapless_architecture/README.md index 7200fa4cd9a7f..d7ad77a39f15b 100644 --- a/planning/mapless_architecture/README.md +++ b/planning/mapless_architecture/README.md @@ -13,9 +13,11 @@ A detailed overview can be seen here: The Mission Planner consists of several components (ROS2 packages) working together: -- **Mission Planner**: Generates target lanes based on the mission input. +- **Local Mission Planner**: Generates target lanes based on the mission input. - **HMI (Human Machine Interface)**: Provides a user interface for defining missions via terminal input. - **Converter**: Converts lanes generated by the Mission Planner into Autoware Trajectories/Paths. +- **Local Road Provider**: Converts the LaneletsStamped message into a RoadSegments message. +- **Local Map Provider**: Converts the RoadSegments message into a LocalMap message. ## Launching the Software diff --git a/planning/mapless_architecture/autoware_local_map_provider/Readme.md b/planning/mapless_architecture/autoware_local_map_provider/Readme.md index 251178ec6cac8..4987fc18d7874 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/Readme.md +++ b/planning/mapless_architecture/autoware_local_map_provider/Readme.md @@ -1,3 +1,3 @@ # Local Map Provider Node -This node converts the mission_planner_messages::msg::RoadSegments message into a mission_planner_messages::msg::LocalMap message right now. More functionality can be added later. +This node converts the RoadSegments message into a LocalMap message right now. More functionality can be added later. diff --git a/planning/mapless_architecture/autoware_local_road_provider/Readme.md b/planning/mapless_architecture/autoware_local_road_provider/Readme.md index 147f05155c78f..82e4949927752 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/Readme.md +++ b/planning/mapless_architecture/autoware_local_road_provider/Readme.md @@ -1,3 +1,3 @@ # Local Road Provider Node -This node converts the db_msgs::msg::LaneletsStamped message into a mission_planner_messages::msg::RoadSegments message right now. More functionality can be added later. +This node converts the LaneletsStamped message into a RoadSegments message right now. More functionality can be added later. diff --git a/planning/mapless_architecture/images/mapless_architecture.svg b/planning/mapless_architecture/images/mapless_architecture.svg new file mode 100644 index 0000000000000..496fd4fd9fb4d --- /dev/null +++ b/planning/mapless_architecture/images/mapless_architecture.svg @@ -0,0 +1,776 @@ + + + + + + + +
+
+
+ Goal Information +
+
+
+ + Goal Information + + + + + + +
+
+
+ Autoware Local Planner +
+
+
+ + Autoware Local Planner + + + + + + +
+
+
+ Sensors +
+
+
+ + Sensors + + + + + + + + +
+
+
/sensing/images &
/sensing/point_clouds & /sensing/odometry
[sensor_msgs::msg::Image &
sensor_msgs::msg::PointCloud2 & nav_msgs::msg::Odometry]
+
+
+ + /sensing/images &... + + + + + + +
+
+
+ safety driver +
+
+
+ + safety driv... + + + + + + + +
+
+
+ to be developed/adapted +
+
+
+ + to be developed/adapted + + + + + + + +
+
+
+ re-used +
+
+
+ + re-used + + + + + + + +
+
+
+ Local Road Provider +
+
+
+ + Local Road Provider + + + + + + +
+
+
+ Autoware +
+ Local Map Provider +
+
+
+ + Autoware... + + + + + + +
+
+
+ Autoware +
+ Local Mission Planner +
+
+
+ + Autoware... + + + + + + +
+
+
+ Autoware +
+ Motion Planner +
+
+
+ + Autoware... + + + + + + +
+
+
+ Autoware +
+ Control +
+
+
+ + Autoware... + + + + + + +
+
+
+ /autoware/planning/mission_planning/mission +
+ [autoware_auto_planning_msgs::msg::LocalMission] +
+
+
+ + /autoware/planning/mission_planning/mission... + + + + + + + + + + + + + + +
+
+
+ /autoware/planning/mission_planning/lanes +
+ [autoware_auto_planning_msgs::msg::MissionLanes] +
+
+
+ + /autoware/planning/mission_planning/lanes... + + + + + + +
+
+
+ /autoware/map/road_segments +
+ [autoware_auto_map_msgs::msg::RoadSegments] +
+
+
+ + /autoware/map/road_segments... + + + + + + +
+
+
+ /autoware/map/local_map +
+ [autoware_auto_map_msgs::msg::LocalMap] +
+
+
+ + /autoware/map/local_map... + + + + + + +
+
+
+ /autoware/planning/scenario_planning/trajectory +
+ [autoware_auto_planning_msgs::msg::Trajectory] +
+
+
+ + /autoware/planning/scenario_planning/trajector... + + + + + + +
+
+
+ Autoware +
+ Local Behavior Planner +
+
+
+ + Autoware... + + + + + + + + +
+
+
+ /autoware/planning/scenario_planning/reference_trajectory +
+ [autoware_auto_planning_msgs::msg::Trajectory] +
+
+
+ + /autoware/planning/scenario_planning/reference_trajec... + + + + + + +
+
+
+
+ + Proposed Architecture + +
+
+
+
+ + Proposed Architecture + + + + + + +
+
+
+ Autoware +
+ Object Detection +
+
+
+ + Autoware... + + + + + + +
+
+
+ Autoware +
+ HMI +
+
+
+ + Autoware... + + + + + + +
+
+
+ Autoware +
+ Sign Detection +
+
+
+ + Autoware... + + + + + + + + +
+
+
+ OpenStreetMaps / +
+ HD Maps +
+
+
+ + OpenStreetMaps /... + + + + + + + + + +
+
+
+ planned for a future release +
+
+
+ + planned for a future relea... + + + + + + +
+
+
+ /autoware/planning/mission_planning/mission +
+ [autoware_auto_planning_msgs::msg::LocalMission] +
+
+
+ + /autoware/planning/mission_planning/mission... + + + + + + +
+
+
+
+ + mission to be executed + +
+
+ enum mission_type (e.g., LaneKeep, TakeNextExit, StopOnShoulder) +
+ float deadline (distance) +
+
+
+ + mission to be executed... + + + + + + +
+
+
+ /autoware/map/local_map +
+ [autoware_auto_map_msgs::msg::LocalMap] +
+
+
+ + /autoware/map/local_map... + + + + + + +
+
+
+ + local map of the vehicle environment + +
+ + + autoware_auto_perception_msgs::msg::DetectedSigns + detected signs + +
+ + vector<DrivingCorridor> driving_corridors
+ DrivingCorridor { +
vector<geometry_msgs::msg::Pose> centerline
+
+ v + ector<geometry_msgs::msg::Pose> bound_left
+
+ v + ector<geometry_msgs::msg::Pose> bound_right
+
enum type (e.g., Road, HighwayExit, Shoulder)
+ } +
+
+
+
+
+ + local map of the vehicle environmentautoware_auto_perception_msgs::msg:... + + + + + + +
+
+
+ /autoware/planning/mission_planning/lanes +
+ [autoware_auto_planning_msgs::msg::MissionLanes] +
+
+
+ + /autoware/planning/mission_planning/lanes... + + + + + + +
+
+
+
+ + + drivable lanes + + +
+
+ uint16 target_lane (local target lane id) +
+
+
+ vector<DrivingCorridor> drivable_lanes  +
+
+ float deadline (distance) +
+
+
+
+ + drivable lanes... + + + + + + + +
+
+
+
+ + Message Definitions + +
+
+
+
+ + Message Definitions + + + + + + + + +
+
+
+ Goal Planner +
+
+
+ + Goal Planner + + + + + + + + + + +
+
+
+ /autoware/map/road_segments +
+ [autoware_auto_map_msgs::msg::RoadSegment] +
+
+
+ + /autoware/map/road_segments... + + + + + + +
+
+
+
+ + + detected road segments + + +
+
+
+
+ + detected road segments + + + + + + + Text is not SVG - cannot display + + + From 5e6ca6816516efb7b58152f8329261c9dc746fc5 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 13 Jun 2024 13:49:05 +0200 Subject: [PATCH 04/24] Add local mission planner Signed-off-by: Simon Eisenmann --- planning/.pages | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/.pages b/planning/.pages index d072126b506e8..0b9bb27181c1b 100644 --- a/planning/.pages +++ b/planning/.pages @@ -40,6 +40,7 @@ nav: - 'Algorithm': planning/autoware_freespace_planning_algorithms - 'RRT*': planning/autoware_freespace_planning_algorithms/rrtstar - 'Mission Planner': planning/autoware_mission_planner + - 'Local Mission Planner': planning/mapless_architecture - 'Motion Planning': - 'Path Optimizer': - 'About Path Optimizer': planning/autoware_path_optimizer From c25cac82c44d013d8915aca4bd8280efd93eef40 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 20 Jun 2024 08:23:07 +0200 Subject: [PATCH 05/24] Remove driveblocks dependencies Signed-off-by: Simon Eisenmann --- .../autoware_hmi/include/hmi_node.hpp | 4 +- .../autoware_hmi/package.xml | 2 +- .../autoware_hmi/src/hmi_node.cpp | 16 +- .../autoware_local_map_provider/Readme.md | 2 +- .../include/local_map_provider_node.hpp | 12 +- .../autoware_local_map_provider/package.xml | 2 +- .../src/local_map_provider_node.cpp | 8 +- .../CMakeLists.txt | 1 - .../include/mission_planner_node.hpp | 65 ++--- .../package.xml | 3 +- .../src/mission_planner_node.cpp | 116 ++++---- .../test/src/test_mission_planner_core.cpp | 87 +++--- .../autoware_local_road_provider/Readme.md | 2 +- .../include/local_road_provider_node.hpp | 4 +- .../autoware_local_road_provider/package.xml | 2 +- .../src/local_road_provider_node.cpp | 4 +- .../include/mission_lane_converter_node.hpp | 8 +- .../package.xml | 2 +- .../src/mission_lane_converter_node.cpp | 6 +- .../src/test_mission_planner_converter.cpp | 10 +- .../CMakeLists.txt | 10 +- .../lib_mission_planner/helper_functions.hpp | 182 +++++++++++- .../local_mission_planner_common/package.xml | 3 +- .../src/helper_functions.cpp | 272 +++++++++++++++++- 24 files changed, 630 insertions(+), 193 deletions(-) diff --git a/planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp index 9323a17b0c86a..f17bc6c47df56 100644 --- a/planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp +++ b/planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp @@ -3,7 +3,7 @@ #ifndef HMI_NODE_HPP_ #define HMI_NODE_HPP_ -#include "mission_planner_messages/msg/mission.hpp" +#include "autoware_planning_msgs/msg/mission.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -47,7 +47,7 @@ class HMINode : public rclcpp::Node // ########################################################################### // Declare ROS2 publisher and subscriber - rclcpp::Publisher::SharedPtr mission_publisher_; + rclcpp::Publisher::SharedPtr mission_publisher_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; }; diff --git a/planning/mapless_architecture/autoware_hmi/package.xml b/planning/mapless_architecture/autoware_hmi/package.xml index 4ffb70df35878..fc306c7dfc175 100644 --- a/planning/mapless_architecture/autoware_hmi/package.xml +++ b/planning/mapless_architecture/autoware_hmi/package.xml @@ -11,8 +11,8 @@ ros2launch - mission_planner_messages rclcpp + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index f24ba77a56167..0d8e9079508c2 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -3,7 +3,7 @@ #include "hmi_node.hpp" -#include "mission_planner_messages/msg/mission.hpp" +#include "autoware_planning_msgs/msg/mission.hpp" #include "rclcpp/rclcpp.hpp" using std::placeholders::_1; @@ -29,7 +29,7 @@ HMINode::HMINode() : Node("hmi_node") // Initialize publisher mission_publisher_ = - this->create_publisher("hmi_node/output/mission", 1); + this->create_publisher("hmi_node/output/mission", 1); // Initialize parameters callback handle param_callback_handle_ = this->add_on_set_parameters_callback( @@ -64,17 +64,17 @@ rcl_interfaces::msg::SetParametersResult HMINode::ParamCallback_( void HMINode::PublishMission_(std::string mission) { - mission_planner_messages::msg::Mission missionMessage; + autoware_planning_msgs::msg::Mission missionMessage; if (mission == "LANE_KEEP") { - missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_KEEP; + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_KEEP; } else if (mission == "LANE_CHANGE_LEFT") { - missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_LEFT; + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_CHANGE_LEFT; } else if (mission == "LANE_CHANGE_RIGHT") { - missionMessage.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_RIGHT; + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_CHANGE_RIGHT; } else if (mission == "TAKE_NEXT_EXIT_LEFT") { - missionMessage.mission_type = mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_LEFT; + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_LEFT; } else if (mission == "TAKE_NEXT_EXIT_RIGHT") { - missionMessage.mission_type = mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_RIGHT; + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_RIGHT; } // TODO(simon.eisenmann@driveblocks.ai): Change deadline parameter diff --git a/planning/mapless_architecture/autoware_local_map_provider/Readme.md b/planning/mapless_architecture/autoware_local_map_provider/Readme.md index 4987fc18d7874..251178ec6cac8 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/Readme.md +++ b/planning/mapless_architecture/autoware_local_map_provider/Readme.md @@ -1,3 +1,3 @@ # Local Map Provider Node -This node converts the RoadSegments message into a LocalMap message right now. More functionality can be added later. +This node converts the mission_planner_messages::msg::RoadSegments message into a mission_planner_messages::msg::LocalMap message right now. More functionality can be added later. diff --git a/planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp index 9055d309cfc57..915da1aa1edee 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp @@ -3,8 +3,8 @@ #ifndef LOCAL_MAP_PROVIDER_NODE_HPP_ #define LOCAL_MAP_PROVIDER_NODE_HPP_ -#include "mission_planner_messages/msg/local_map.hpp" -#include "mission_planner_messages/msg/road_segments.hpp" +#include "autoware_planning_msgs/msg/local_map.hpp" +#include "autoware_planning_msgs/msg/road_segments.hpp" #include "rclcpp/rclcpp.hpp" /** @@ -29,18 +29,18 @@ class LocalMapProviderNode : public rclcpp::Node /** * @brief The callback for the RoadSegments messages. * - * @param msg The mission_planner_messages::msg::RoadSegments message. + * @param msg The autoware_planning_msgs::msg::RoadSegments message. */ - void CallbackRoadSegmentsMessages_(const mission_planner_messages::msg::RoadSegments & msg); + void CallbackRoadSegmentsMessages_(const autoware_planning_msgs::msg::RoadSegments & msg); // ########################################################################### // # PRIVATE VARIABLES // ########################################################################### // Declare ROS2 publisher and subscriber - rclcpp::Publisher::SharedPtr map_publisher_; + rclcpp::Publisher::SharedPtr map_publisher_; - rclcpp::Subscription::SharedPtr road_subscriber_; + rclcpp::Subscription::SharedPtr road_subscriber_; }; #endif // LOCAL_MAP_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_map_provider/package.xml b/planning/mapless_architecture/autoware_local_map_provider/package.xml index cf8a6a476bbc8..f2464d374d5d4 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_map_provider/package.xml @@ -13,8 +13,8 @@ db_msgs lib_mission_planner - mission_planner_messages rclcpp + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp index b6f4a900ce659..8f296ddc36dd4 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -16,19 +16,19 @@ LocalMapProviderNode::LocalMapProviderNode() : Node("local_map_provider_node") qos.best_effort(); // Initialize publisher for local map - map_publisher_ = this->create_publisher( + map_publisher_ = this->create_publisher( "local_map_provider_node/output/local_map", 1); // Initialize subscriber to road segments messages - road_subscriber_ = this->create_subscription( + road_subscriber_ = this->create_subscription( "local_map_provider_node/input/road_segments", qos, std::bind(&LocalMapProviderNode::CallbackRoadSegmentsMessages_, this, _1)); } void LocalMapProviderNode::CallbackRoadSegmentsMessages_( - const mission_planner_messages::msg::RoadSegments & msg) + const autoware_planning_msgs::msg::RoadSegments & msg) { - mission_planner_messages::msg::LocalMap local_map; + autoware_planning_msgs::msg::LocalMap local_map; // Save road segments in the local map message local_map.road_segments = msg; diff --git a/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt index 361c943fa79e7..0558617dcb323 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt @@ -39,7 +39,6 @@ set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) - find_package(mission_planner_messages REQUIRED) ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp test/src/test_mission_planner_core.cpp diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp index 2eaed7604fdb8..644aa23eac40e 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp @@ -4,15 +4,12 @@ #define MISSION_PLANNER_NODE_HPP_ #include "lanelet2_core/geometry/LineString.h" -#include "lanelet_helper/lanelet_converter.hpp" -#include "lanelet_helper/lanelet_geometry.hpp" -#include "lanelet_helper/lanelet_tools.hpp" #include "lib_mission_planner/helper_functions.hpp" -#include "mission_planner_messages/msg/driving_corridor.hpp" -#include "mission_planner_messages/msg/local_map.hpp" -#include "mission_planner_messages/msg/mission.hpp" -#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" -#include "mission_planner_messages/msg/visualization_distance.hpp" +#include "autoware_planning_msgs/msg/driving_corridor.hpp" +#include "autoware_planning_msgs/msg/local_map.hpp" +#include "autoware_planning_msgs/msg/mission.hpp" +#include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" +#include "autoware_planning_msgs/msg/visualization_distance.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -103,13 +100,13 @@ class MissionPlannerNode : public rclcpp::Node * @param converted_lanelets The lanelets from the road model (std::vector). * @param lanelet_connections The lanelet connections from the road model - (std::vector). + (std::vector). * @return bool (is on goal lane or not). */ bool IsOnGoalLane_( const int ego_lanelet_index, const lanelet::BasicPoint2d & goal_point, const std::vector & converted_lanelets, - const std::vector & lanelet_connections); + const std::vector & lanelet_connections); /** * @brief Function which checks if the goal point has a negative x value und @@ -119,11 +116,11 @@ class MissionPlannerNode : public rclcpp::Node * @param converted_lanelets The lanelets from the road model (std::vector). * @param lanelet_connections The lanelet connections from the road model - (std::vector). + (std::vector). */ void CheckIfGoalPointShouldBeReset_( const lanelet::Lanelets & converted_lanelets, - const std::vector & lanelet_connections); + const std::vector & lanelet_connections); /** * @brief Function for calculating lanes @@ -135,7 +132,7 @@ class MissionPlannerNode : public rclcpp::Node */ Lanes CalculateLanes_( const std::vector & converted_lanelets, - std::vector & lanelet_connections); + std::vector & lanelet_connections); /** * @brief Function for creating a marker array. @@ -152,7 +149,7 @@ class MissionPlannerNode : public rclcpp::Node const std::vector & centerline, const std::vector & left, const std::vector & right, - const mission_planner_messages::msg::RoadSegments & msg); + const autoware_planning_msgs::msg::RoadSegments & msg); /** * @brief Getter for goal_point_ @@ -174,36 +171,36 @@ class MissionPlannerNode : public rclcpp::Node * @param lane The lane which is a std::vector containing all the indices * of the lane. * @param converted_lanelets The lanelets (std::vector). - * @return mission_planner_messages::msg::DrivingCorridor + * @return autoware_planning_msgs::msg::DrivingCorridor */ - mission_planner_messages::msg::DrivingCorridor CreateDrivingCorridor_( + autoware_planning_msgs::msg::DrivingCorridor CreateDrivingCorridor_( const std::vector & lane, const std::vector & converted_lanelets); /** * @brief The callback for the Mission messages. * - * @param msg The mission_planner_messages::msg::Mission message. + * @param msg The autoware_planning_msgs::msg::Mission message. */ - void CallbackMissionMessages_(const mission_planner_messages::msg::Mission & msg); + void CallbackMissionMessages_(const autoware_planning_msgs::msg::Mission & msg); /** * @brief The callback for the LocalMap messages. * - * @param msg The mission_planner_messages::msg::LocalMap message. + * @param msg The autoware_planning_msgs::msg::LocalMap message. */ - void CallbackLocalMapMessages_(const mission_planner_messages::msg::LocalMap & msg); + void CallbackLocalMapMessages_(const autoware_planning_msgs::msg::LocalMap & msg); /** * @brief Convert RoadSegments into lanelets. * - * @param msg The message (mission_planner_messages::msg::RoadSegments). + * @param msg The message (autoware_planning_msgs::msg::RoadSegments). * @param out_lanelets The lanelets (output). * @param out_lanelet_connections The lanelet connections (output). */ void ConvertInput2LaneletFormat( - const mission_planner_messages::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::RoadSegments & msg, std::vector & out_lanelets, - std::vector & out_lanelet_connections); + std::vector & out_lanelet_connections); private: // ########################################################################### @@ -217,7 +214,7 @@ class MissionPlannerNode : public rclcpp::Node * @param converted_lanelets The lanelets (std::vector). */ void VisualizeLanes_( - const mission_planner_messages::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::RoadSegments & msg, const std::vector & converted_lanelets); /** @@ -227,8 +224,8 @@ class MissionPlannerNode : public rclcpp::Node * @param driving_corridor The considered driving corridor for which the centerline is visualized. */ void VisualizeCenterlineOfDrivingCorridor_( - const mission_planner_messages::msg::RoadSegments & msg, - const mission_planner_messages::msg::DrivingCorridor & driving_corridor); + const autoware_planning_msgs::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::DrivingCorridor & driving_corridor); /** * @brief Function for creating a lanelet::LineString2d. @@ -264,8 +261,7 @@ class MissionPlannerNode : public rclcpp::Node * @return std::vector */ std::vector GetAllNeighborsOfLane( - const std::vector & lane, - const std::vector & lanelet_connections, + const std::vector & lane, const std::vector & lanelet_connections, const int vehicle_side); /** @@ -277,35 +273,34 @@ class MissionPlannerNode : public rclcpp::Node * */ void InsertPredecessorLanelet( - std::vector & lane_idx, - const std::vector & lanelet_connections); + std::vector & lane_idx, const std::vector & lanelet_connections); /** * @brief Calculate the predecessors. * * @param lanelet_connections The lanelet connections. */ - void CalculatePredecessors(std::vector & lanelet_connections); + void CalculatePredecessors(std::vector & lanelet_connections); // ########################################################################### // # PRIVATE VARIABLES // ########################################################################### // Declare ROS2 publisher and subscriber - rclcpp::Subscription::SharedPtr mapSubscriber_; + rclcpp::Subscription::SharedPtr mapSubscriber_; - rclcpp::Subscription::SharedPtr missionSubscriber_; + rclcpp::Subscription::SharedPtr missionSubscriber_; rclcpp::Publisher::SharedPtr visualizationPublisher_; rclcpp::Publisher::SharedPtr visualization_publisher_centerline_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr visualizationDistancePublisher_; rclcpp::Publisher::SharedPtr visualizationGoalPointPublisher_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr missionLanesStampedPublisher_; rclcpp::Subscription::SharedPtr OdometrySubscriber_; diff --git a/planning/mapless_architecture/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/autoware_local_mission_planner/package.xml index c2314663d155f..1301143db8217 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner/package.xml @@ -15,14 +15,13 @@ db_msgs geometry_msgs lanelet2_core - lanelet_helper lib_mission_planner - mission_planner_messages rclcpp tf2 tf2_geometry_msgs tf2_ros visualization_msgs + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 94d117666dd4c..4c10d995e42e2 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -5,14 +5,10 @@ #include "lanelet2_core/LaneletMap.h" #include "lanelet2_core/geometry/Lanelet.h" #include "lanelet2_core/geometry/LineString.h" -#include "lanelet_helper/data_types.hpp" -#include "lanelet_helper/lanelet_converter.hpp" -#include "lanelet_helper/lanelet_geometry.hpp" -#include "lanelet_helper/lanelet_tools.hpp" #include "lib_mission_planner/helper_functions.hpp" -#include "mission_planner_messages/msg/driving_corridor.hpp" -#include "mission_planner_messages/msg/mission.hpp" -#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" +#include "autoware_planning_msgs/msg/driving_corridor.hpp" +#include "autoware_planning_msgs/msg/mission.hpp" +#include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" #include "rclcpp/rclcpp.hpp" #include "db_msgs/msg/lanelets_stamped.hpp" @@ -53,7 +49,7 @@ MissionPlannerNode::MissionPlannerNode() : Node("mission_planner_node") // Initialize publisher for visualization of the distance visualizationDistancePublisher_ = - this->create_publisher( + this->create_publisher( "mission_planner_node/output/visualization_distance", 1); // Initialize publisher for goal point marker @@ -62,16 +58,16 @@ MissionPlannerNode::MissionPlannerNode() : Node("mission_planner_node") // Initialize publisher for mission lanes missionLanesStampedPublisher_ = - this->create_publisher( + this->create_publisher( "mission_planner_node/output/mission_lanes_stamped", 1); // Initialize subscriber to lanelets stamped messages - mapSubscriber_ = this->create_subscription( + mapSubscriber_ = this->create_subscription( "mission_planner_node/input/local_map", qos, std::bind(&MissionPlannerNode::CallbackLocalMapMessages_, this, _1)); // Initialize subscriber to mission messages - missionSubscriber_ = this->create_subscription( + missionSubscriber_ = this->create_subscription( "mission_planner/input/mission", qos, std::bind(&MissionPlannerNode::CallbackMissionMessages_, this, _1)); @@ -104,10 +100,10 @@ MissionPlannerNode::MissionPlannerNode() : Node("mission_planner_node") } void MissionPlannerNode::CallbackLocalMapMessages_( - const mission_planner_messages::msg::LocalMap & msg) + const autoware_planning_msgs::msg::LocalMap & msg) { // Used for output - std::vector lanelet_connections; + std::vector lanelet_connections; std::vector converted_lanelets; ConvertInput2LaneletFormat(msg.road_segments, converted_lanelets, lanelet_connections); @@ -159,8 +155,7 @@ void MissionPlannerNode::CallbackLocalMapMessages_( // Check if lane change was successful, if yes -> reset mission if (mission_ != stay) { - int egoLaneletIndex = - path_geometry::FindEgoOccupiedLaneletID(converted_lanelets); // Returns -1 if no match + int egoLaneletIndex = FindEgoOccupiedLaneletID(converted_lanelets); // Returns -1 if no match lanelet::BasicPoint2d pointEgo(0, 0); // Vehicle is always located at (0, 0) @@ -196,7 +191,7 @@ void MissionPlannerNode::CallbackLocalMapMessages_( visualization_publisher_centerline_->publish(clear_marker_array); // Publish mission lanes - mission_planner_messages::msg::MissionLanesStamped lanes; + autoware_planning_msgs::msg::MissionLanesStamped lanes; lanes.header.frame_id = msg.road_segments.header.frame_id; // Same frame_id as msg lanes.header.stamp = rclcpp::Node::now(); @@ -231,7 +226,7 @@ void MissionPlannerNode::CallbackLocalMapMessages_( MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_(msg.road_segments, lanes.ego_lane); // Initialize driving corridor - mission_planner_messages::msg::DrivingCorridor driving_corridor; + autoware_planning_msgs::msg::DrivingCorridor driving_corridor; if (!left_lanes.empty()) { for (const std::vector & lane : left_lanes) { @@ -441,7 +436,7 @@ lanelet::BasicPoint2d MissionPlannerNode::RecenterGoalPoint( lanelet::BasicPoint2d projected_goal_point; // Get current lanelet index of goal point - const int lanelet_idx_goal_point = path_geometry::FindOccupiedLaneletID(road_model, goal_point); + const int lanelet_idx_goal_point = FindOccupiedLaneletID(road_model, goal_point); if (lanelet_idx_goal_point >= 0) { // Get the centerline of the goal point's lanelet @@ -459,36 +454,36 @@ lanelet::BasicPoint2d MissionPlannerNode::RecenterGoalPoint( } void MissionPlannerNode::CallbackMissionMessages_( - const mission_planner_messages::msg::Mission & msg) + const autoware_planning_msgs::msg::Mission & msg) { // Initialize variables lane_change_trigger_success_ = false; retry_attempts_ = 0; switch (msg.mission_type) { - case mission_planner_messages::msg::Mission::LANE_KEEP: + case autoware_planning_msgs::msg::Mission::LANE_KEEP: // Keep the lane mission_ = stay; target_lane_ = stay; break; - case mission_planner_messages::msg::Mission::LANE_CHANGE_LEFT: + case autoware_planning_msgs::msg::Mission::LANE_CHANGE_LEFT: // Initiate left lane change RCLCPP_INFO(this->get_logger(), "Lane change to the left initiated."); lane_change_direction_ = left; InitiateLaneChange_(lane_change_direction_, lane_left_); break; - case mission_planner_messages::msg::Mission::LANE_CHANGE_RIGHT: + case autoware_planning_msgs::msg::Mission::LANE_CHANGE_RIGHT: // Initiate right lane change RCLCPP_INFO(this->get_logger(), "Lane change to the right initiated."); lane_change_direction_ = right; InitiateLaneChange_(lane_change_direction_, lane_right_); break; - case mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_LEFT: + case autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_LEFT: // Initiate take next exit RCLCPP_INFO(this->get_logger(), "Take next exit (left) initiated."); target_lane_ = left_most; // Set target lane break; - case mission_planner_messages::msg::Mission::TAKE_NEXT_EXIT_RIGHT: + case autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_RIGHT: // Initiate take next exit RCLCPP_INFO(this->get_logger(), "Take next exit (right) initiated."); target_lane_ = right_most; // Set target lane @@ -520,7 +515,7 @@ void MissionPlannerNode::InitiateLaneChange_( } void MissionPlannerNode::VisualizeLanes_( - const mission_planner_messages::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::RoadSegments & msg, const std::vector & converted_lanelets) { // Calculate centerlines, left and right bounds @@ -547,8 +542,8 @@ void MissionPlannerNode::VisualizeLanes_( } void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( - const mission_planner_messages::msg::RoadSegments & msg, - const mission_planner_messages::msg::DrivingCorridor & driving_corridor) + const autoware_planning_msgs::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::DrivingCorridor & driving_corridor) { // Create a marker for the centerline visualization_msgs::msg::Marker centerline_marker; @@ -592,16 +587,16 @@ void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( // Determine the lanes Lanes MissionPlannerNode::CalculateLanes_( const std::vector & converted_lanelets, - std::vector & lanelet_connections) + std::vector & lanelet_connections) { // Calculate centerlines, left and right bounds std::vector centerlines; std::vector left_bounds; std::vector right_bounds; - int ego_lanelet_index = path_geometry::FindEgoOccupiedLaneletID( - converted_lanelets); // Finds the ID of the ego vehicle occupied - // lanelet (returns -1 if no match) + int ego_lanelet_index = + FindEgoOccupiedLaneletID(converted_lanelets); // Finds the ID of the ego vehicle occupied + // lanelet (returns -1 if no match) // Initialize variables std::vector> ego_lane; @@ -611,23 +606,23 @@ Lanes MissionPlannerNode::CalculateLanes_( if (ego_lanelet_index >= 0) { // Get ego lane - ego_lane = lanelet_tools::GetAllSuccessorSequences(lanelet_connections, ego_lanelet_index); + ego_lane = GetAllSuccessorSequences(lanelet_connections, ego_lanelet_index); // Extract the first available ego lane if (ego_lane.size() > 0) { ego_lane_stripped_idx = ego_lane[0]; // Get all neighbor lanelets to the ego lanelet on the left side - std::vector left_neighbors = lanelet_tools::GetAllNeighboringLaneletIDs( - lanelet_connections, ego_lanelet_index, lanelet_types::VehicleSide::kLeft); + std::vector left_neighbors = + GetAllNeighboringLaneletIDs(lanelet_connections, ego_lanelet_index, VehicleSide::kLeft); // Initialize current_lane and next_lane std::vector current_lane = ego_lane_stripped_idx; std::vector neighbor_lane; for (size_t i = 0; i < left_neighbors.size(); ++i) { - neighbor_lane = GetAllNeighborsOfLane( - current_lane, lanelet_connections, lanelet_types::VehicleSide::kLeft); + neighbor_lane = + GetAllNeighborsOfLane(current_lane, lanelet_connections, VehicleSide::kLeft); left_lanes.push_back(neighbor_lane); @@ -635,15 +630,15 @@ Lanes MissionPlannerNode::CalculateLanes_( } // Get all neighbor lanelets to the ego lanelet on the right side - std::vector right_neighbors = lanelet_tools::GetAllNeighboringLaneletIDs( - lanelet_connections, ego_lanelet_index, lanelet_types::VehicleSide::kRight); + std::vector right_neighbors = + GetAllNeighboringLaneletIDs(lanelet_connections, ego_lanelet_index, VehicleSide::kRight); // Reinitialize current_lane current_lane = ego_lane_stripped_idx; for (size_t i = 0; i < right_neighbors.size(); ++i) { - neighbor_lane = GetAllNeighborsOfLane( - current_lane, lanelet_connections, lanelet_types::VehicleSide::kRight); + neighbor_lane = + GetAllNeighborsOfLane(current_lane, lanelet_connections, VehicleSide::kRight); right_lanes.push_back(neighbor_lane); @@ -675,8 +670,7 @@ Lanes MissionPlannerNode::CalculateLanes_( } void MissionPlannerNode::InsertPredecessorLanelet( - std::vector & lane_idx, - const std::vector & lanelet_connections) + std::vector & lane_idx, const std::vector & lanelet_connections) { if (!lane_idx.empty()) { // Get index of first lanelet @@ -696,8 +690,8 @@ void MissionPlannerNode::InsertPredecessorLanelet( } std::vector MissionPlannerNode::GetAllNeighborsOfLane( - const std::vector & lane, - const std::vector & lanelet_connections, const int vehicle_side) + const std::vector & lane, const std::vector & lanelet_connections, + const int vehicle_side) { // Initialize vector std::vector neighbor_lane_idx = {}; @@ -730,7 +724,7 @@ visualization_msgs::msg::MarkerArray MissionPlannerNode::CreateMarkerArray_( const std::vector & centerline, const std::vector & left, const std::vector & right, - const mission_planner_messages::msg::RoadSegments & msg) + const autoware_planning_msgs::msg::RoadSegments & msg) { visualization_msgs::msg::MarkerArray markerArray; @@ -810,11 +804,11 @@ visualization_msgs::msg::MarkerArray MissionPlannerNode::CreateMarkerArray_( return markerArray; } -mission_planner_messages::msg::DrivingCorridor MissionPlannerNode::CreateDrivingCorridor_( +autoware_planning_msgs::msg::DrivingCorridor MissionPlannerNode::CreateDrivingCorridor_( const std::vector & lane, const std::vector & converted_lanelets) { // Create driving corridor - mission_planner_messages::msg::DrivingCorridor driving_corridor; + autoware_planning_msgs::msg::DrivingCorridor driving_corridor; for (int id : lane) { if (id >= 0) { @@ -895,16 +889,15 @@ lanelet::BasicPoint2d MissionPlannerNode::GetPointOnLane_( bool MissionPlannerNode::IsOnGoalLane_( const int ego_lanelet_index, const lanelet::BasicPoint2d & goal_point, const std::vector & converted_lanelets, - const std::vector & lanelet_connections) + const std::vector & lanelet_connections) { bool result = false; // Find the index of the lanelet containing the goal point - int goal_index = - path_geometry::FindOccupiedLaneletID(converted_lanelets, goal_point); // Returns -1 if no match + int goal_index = FindOccupiedLaneletID(converted_lanelets, goal_point); // Returns -1 if no match if (goal_index >= 0) { // Check if -1 - std::vector> goal_lane = lanelet_tools::GetAllPredecessorSequences( + std::vector> goal_lane = GetAllPredecessorSequences( lanelet_connections, goal_index); // Get goal lane @@ -954,7 +947,7 @@ double MissionPlannerNode::CalculateDistanceBetweenPointAndLineString_( double distance = lanelet::geometry::distance2d(point, projected_point); // Publish distance - mission_planner_messages::msg::VisualizationDistance d; + autoware_planning_msgs::msg::VisualizationDistance d; d.distance = distance; visualizationDistancePublisher_->publish(d); @@ -963,20 +956,20 @@ double MissionPlannerNode::CalculateDistanceBetweenPointAndLineString_( void MissionPlannerNode::CheckIfGoalPointShouldBeReset_( const lanelet::Lanelets & converted_lanelets, - const std::vector & lanelet_connections) + const std::vector & lanelet_connections) { // Check if goal point should be reset: If the x value of the goal point is // negative, then the point is behind the vehicle and must be therefore reset. if (goal_point_.x() < 0 && mission_ != stay) { // TODO(simon.eisenmann@driveblocks.ai): Maybe // remove condition mission_ != stay // Find the index of the lanelet containing the goal point - int goal_index = path_geometry::FindOccupiedLaneletID( - converted_lanelets, goal_point_); // Returns -1 if no match + int goal_index = + FindOccupiedLaneletID(converted_lanelets, goal_point_); // Returns -1 if no match if (goal_index >= 0) { // Check if -1 // Reset goal point goal_point_ = GetPointOnLane_( - lanelet_tools::GetAllSuccessorSequences(lanelet_connections, goal_index)[0], + GetAllSuccessorSequences(lanelet_connections, goal_index)[0], projection_distance_on_goallane_, converted_lanelets); } else { // Reset of goal point not successful -> reset mission and target lane @@ -1001,9 +994,9 @@ void MissionPlannerNode::goal_point(const lanelet::BasicPoint2d & goal_point) } void MissionPlannerNode::ConvertInput2LaneletFormat( - const mission_planner_messages::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::RoadSegments & msg, std::vector & out_lanelets, - std::vector & out_lanelet_connections) + std::vector & out_lanelet_connections) { // Local variables const unsigned int n_linestrings_per_lanelet = 2; @@ -1047,7 +1040,7 @@ void MissionPlannerNode::ConvertInput2LaneletFormat( out_lanelets.push_back(lanelet); // Add empty lanelet connection - out_lanelet_connections.push_back(lanelet_types::LaneletConnection()); + out_lanelet_connections.push_back(LaneletConnection()); // Set origin lanelet ID (and store it in translation map) out_lanelet_connections[idx_segment].original_lanelet_id = msg.segments[idx_segment].id; @@ -1097,8 +1090,7 @@ void MissionPlannerNode::ConvertInput2LaneletFormat( return; } -void MissionPlannerNode::CalculatePredecessors( - std::vector & lanelet_connections) +void MissionPlannerNode::CalculatePredecessors(std::vector & lanelet_connections) { // Determine predecessor information from already known information for (std::size_t id_lanelet = 0; id_lanelet < lanelet_connections.size(); id_lanelet++) { @@ -1118,7 +1110,7 @@ void MissionPlannerNode::CalculatePredecessors( } // Write -1 to lanelets which have no predecessors - for (lanelet_types::LaneletConnection lanelet_connection : lanelet_connections) { + for (LaneletConnection lanelet_connection : lanelet_connections) { if (lanelet_connection.predecessor_lanelet_ids.empty()) { lanelet_connection.predecessor_lanelet_ids = {-1}; } diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index 2a7edb198967e..69463cb49abc4 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -154,15 +154,18 @@ int TestGetPointOnLane() // Create some example lanelets auto msg_lanelets = CreateLanelets(); - // convert road model - LaneletConverter lanelet_converter; - std::vector lanelet_connections; + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); + + // Convert road model + std::vector lanelet_connections; std::vector lanelets; - lanelet_converter.ConvertInput2LaneletFormat(msg_lanelets, lanelets, lanelet_connections); + // Convert message + autoware_planning_msgs::msg::RoadSegments road_segments = + lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg_lanelets); - // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); // Get a point from the tested function which has the x value 3.0 and lies on // the centerline of the lanelets @@ -188,16 +191,20 @@ int TestIsOnGoalLane() { // Create some example lanelets auto msg_road_model = CreateLanelets(); - // convert road model - LaneletConverter lanelet_converter; - std::vector lanelet_connections; - std::vector lanelets; - - lanelet_converter.ConvertInput2LaneletFormat(msg_road_model, lanelets, lanelet_connections); // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); + // Convert road model + std::vector lanelet_connections; + std::vector lanelets; + + // Convert message + autoware_planning_msgs::msg::RoadSegments road_segments = + lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg_road_model); + + MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); + // Define a point with x = 1.0 and y = 0.0 lanelet::BasicPoint2d point(1.0, 0.0); @@ -313,14 +320,16 @@ int TestRecenterGoalpoint() // Get a local road model for testing db_msgs::msg::LaneletsStamped local_road_model = GetTestRoadModelForRecenterTests(); - LaneletConverter lanelet_converter; - // Used for the output - std::vector lanelet_connections; + std::vector lanelet_connections; std::vector converted_lanelets; - lanelet_converter.ConvertInput2LaneletFormat( - local_road_model, converted_lanelets, lanelet_connections); + // Convert message + autoware_planning_msgs::msg::RoadSegments road_segments = + lib_mission_planner::ConvertLaneletsStamped2RoadSegments(local_road_model); + + mission_planner.ConvertInput2LaneletFormat( + road_segments, converted_lanelets, lanelet_connections); // TEST 1: point on centerline of origin lanelet // define a test point and re-center onto its centerline @@ -382,19 +391,18 @@ int TestCheckIfGoalPointShouldBeReset() auto msg = CreateLanelets(); // Convert message - mission_planner_messages::msg::RoadSegments road_segments = + autoware_planning_msgs::msg::RoadSegments road_segments = lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg); - mission_planner_messages::msg::LocalMap local_map; + autoware_planning_msgs::msg::LocalMap local_map; local_map.road_segments = road_segments; - // Convert road model - LaneletConverter lanelet_converter; - std::vector lanelet_connections; - std::vector lanelets; - // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); + // Convert road model + std::vector lanelet_connections; + std::vector lanelets; + MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); // Compute available lanes @@ -406,8 +414,8 @@ int TestCheckIfGoalPointShouldBeReset() MissionPlanner.goal_point(point); // set a non-default mission to make the goal point reset work - mission_planner_messages::msg::Mission mission_msg; - mission_msg.mission_type = mission_planner_messages::msg::Mission::LANE_CHANGE_LEFT; + autoware_planning_msgs::msg::Mission mission_msg; + mission_msg.mission_type = autoware_planning_msgs::msg::Mission::LANE_CHANGE_LEFT; MissionPlanner.CallbackMissionMessages_(mission_msg); // Call function which is tested @@ -423,7 +431,7 @@ int TestCheckIfGoalPointShouldBeReset() MissionPlanner.goal_point(point); // set a non-default mission to make the goal point reset work - mission_msg.mission_type = mission_planner_messages::msg::Mission::LANE_KEEP; + mission_msg.mission_type = autoware_planning_msgs::msg::Mission::LANE_KEEP; MissionPlanner.CallbackMissionMessages_(mission_msg); // Call function which is tested @@ -438,8 +446,7 @@ int TestCheckIfGoalPointShouldBeReset() return 0; } -std::tuple, std::vector> -CreateLane() +std::tuple, std::vector> CreateLane() { // Local variables const int n_lanelets = 2; @@ -522,14 +529,18 @@ CreateLane() message.lanelets[1].successor_lanelet_id = {-1}; message.lanelets[1].neighboring_lanelet_id = {-1, -1}; - // Get converter - LaneletConverter lanelet_converter; + // Initialize MissionPlannerNode + MissionPlannerNode MissionPlanner = MissionPlannerNode(); // Output - std::vector lanelet_connections; + std::vector lanelet_connections; std::vector converted_lanelets; - lanelet_converter.ConvertInput2LaneletFormat(message, converted_lanelets, lanelet_connections); + // Convert message + autoware_planning_msgs::msg::RoadSegments road_segments = + lib_mission_planner::ConvertLaneletsStamped2RoadSegments(message); + + MissionPlanner.ConvertInput2LaneletFormat(road_segments, converted_lanelets, lanelet_connections); return std::make_tuple(converted_lanelets, lanelet_connections); } @@ -539,7 +550,7 @@ int TestCalculateLanes() // 0, the ego lanelet is 0 auto tuple = CreateLane(); std::vector lanelets = std::get<0>(tuple); - std::vector lanelet_connections = std::get<1>(tuple); + std::vector lanelet_connections = std::get<1>(tuple); // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); @@ -575,13 +586,13 @@ int TestCreateMarkerArray() // Create some example lanelets auto tuple = CreateLane(); std::vector lanelets = std::get<0>(tuple); - std::vector lanelet_connections = std::get<1>(tuple); + std::vector lanelet_connections = std::get<1>(tuple); // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); // Create empty message - mission_planner_messages::msg::RoadSegments message; + autoware_planning_msgs::msg::RoadSegments message; // Calculate centerlines, left and right bounds std::vector centerlines; @@ -622,13 +633,13 @@ int TestCreateDrivingCorridor() // Create some example lanelets auto tuple = CreateLane(); std::vector lanelets = std::get<0>(tuple); - std::vector lanelet_connections = std::get<1>(tuple); + std::vector lanelet_connections = std::get<1>(tuple); // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); // Call function which is tested - mission_planner_messages::msg::DrivingCorridor driving_corridor = + autoware_planning_msgs::msg::DrivingCorridor driving_corridor = MissionPlanner.CreateDrivingCorridor_({0, 1}, lanelets); // Check if x value of first point in centerline is -2.0 diff --git a/planning/mapless_architecture/autoware_local_road_provider/Readme.md b/planning/mapless_architecture/autoware_local_road_provider/Readme.md index 82e4949927752..147f05155c78f 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/Readme.md +++ b/planning/mapless_architecture/autoware_local_road_provider/Readme.md @@ -1,3 +1,3 @@ # Local Road Provider Node -This node converts the LaneletsStamped message into a RoadSegments message right now. More functionality can be added later. +This node converts the db_msgs::msg::LaneletsStamped message into a mission_planner_messages::msg::RoadSegments message right now. More functionality can be added later. diff --git a/planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp index e3e0f82f98dc4..786a1fc3f0b4c 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp @@ -3,7 +3,7 @@ #ifndef LOCAL_ROAD_PROVIDER_NODE_HPP_ #define LOCAL_ROAD_PROVIDER_NODE_HPP_ -#include "mission_planner_messages/msg/road_segments.hpp" +#include "autoware_planning_msgs/msg/road_segments.hpp" #include "rclcpp/rclcpp.hpp" #include "db_msgs/msg/lanelets_stamped.hpp" @@ -39,7 +39,7 @@ class LocalRoadProviderNode : public rclcpp::Node // ########################################################################### // Declare ROS2 publisher and subscriber - rclcpp::Publisher::SharedPtr road_publisher_; + rclcpp::Publisher::SharedPtr road_publisher_; rclcpp::Subscription::SharedPtr lanelets_subscriber_; }; diff --git a/planning/mapless_architecture/autoware_local_road_provider/package.xml b/planning/mapless_architecture/autoware_local_road_provider/package.xml index 67e6211a2d361..883ce76a9ddb5 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_road_provider/package.xml @@ -13,8 +13,8 @@ db_msgs lib_mission_planner - mission_planner_messages rclcpp + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp index 1f0e645826b89..9870d85ab9cd6 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp @@ -17,7 +17,7 @@ LocalRoadProviderNode::LocalRoadProviderNode() : Node("local_road_provider_node" qos.best_effort(); // Initialize publisher for road segments - road_publisher_ = this->create_publisher( + road_publisher_ = this->create_publisher( "local_road_provider_node/output/road_segments", 1); // Initialize subscriber to lanelets stamped messages @@ -28,7 +28,7 @@ LocalRoadProviderNode::LocalRoadProviderNode() : Node("local_road_provider_node" void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg) { - mission_planner_messages::msg::RoadSegments road_segments = + autoware_planning_msgs::msg::RoadSegments road_segments = lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg); // Publish the RoadSegments message diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp index 47b1052083e48..3a8d07ff86afe 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp @@ -3,7 +3,7 @@ #ifndef MISSION_LANE_CONVERTER_NODE_HPP_ #define MISSION_LANE_CONVERTER_NODE_HPP_ -#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" +#include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" @@ -41,7 +41,7 @@ class MissionLaneConverterNode : public rclcpp::Node std::tuple< autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> - ConvertMissionToTrajectory(const mission_planner_messages::msg::MissionLanesStamped & msg); + ConvertMissionToTrajectory(const autoware_planning_msgs::msg::MissionLanesStamped & msg); private: // ########################################################################### @@ -53,7 +53,7 @@ class MissionLaneConverterNode : public rclcpp::Node * * @param msg Mission lanes from the mission planner module */ - void MissionLanesCallback_(const mission_planner_messages::msg::MissionLanesStamped & msg); + void MissionLanesCallback_(const autoware_planning_msgs::msg::MissionLanesStamped & msg); /** * @brief Adds a trajectory point to the pre-allocated ROS message. @@ -144,7 +144,7 @@ class MissionLaneConverterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr odom_subscriber_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr mission_lane_subscriber_; rclcpp::Publisher::SharedPtr trajectory_publisher_, diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml index 4942e7b8dce5e..b4b872356dffd 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml +++ b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml @@ -14,10 +14,10 @@ autoware_auto_planning_msgs geometry_msgs lib_mission_planner - mission_planner_messages rclcpp tf2_geometry_msgs visualization_msgs + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 00f7f2456d56b..25a4800aef43c 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -35,7 +35,7 @@ MissionLaneConverterNode::MissionLaneConverterNode() : Node("mission_lane_conver std::bind(&MissionLaneConverterNode::CallbackOdometryMessages_, this, _1)); mission_lane_subscriber_ = - this->create_subscription( + this->create_subscription( "mission_lane_converter/input/mission_lanes", qos_best_effort, std::bind(&MissionLaneConverterNode::MissionLanesCallback_, this, _1)); @@ -108,7 +108,7 @@ void MissionLaneConverterNode::TimedStartupTrajectoryCallback() } void MissionLaneConverterNode::MissionLanesCallback_( - const mission_planner_messages::msg::MissionLanesStamped & msg_mission) + const autoware_planning_msgs::msg::MissionLanesStamped & msg_mission) { // FIXME: workaround to get the vehicle driving in autonomous mode until the // environment model is available @@ -172,7 +172,7 @@ std::tuple< autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> MissionLaneConverterNode::ConvertMissionToTrajectory( - const mission_planner_messages::msg::MissionLanesStamped & msg) + const autoware_planning_msgs::msg::MissionLanesStamped & msg) { // empty trajectory for controller autoware_auto_planning_msgs::msg::Trajectory trj_msg = diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp index b0e4f2bde1825..0b8a9efea8852 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -3,8 +3,6 @@ #include "gtest/gtest.h" #include "mission_lane_converter_node.hpp" -#include "mission_planner_messages/msg/driving_corridor.hpp" -#include "mission_planner_messages/msg/mission_lanes_stamped.hpp" #include "geometry_msgs/msg/point.hpp" @@ -12,13 +10,13 @@ int TestMissionToTrajectory() { MissionLaneConverterNode mission_converter = MissionLaneConverterNode(); - mission_planner_messages::msg::MissionLanesStamped mission_msg; + autoware_planning_msgs::msg::MissionLanesStamped mission_msg; // set target lane to ego lane mission_msg.target_lane = 0; // add a driving corridor to the ego lane - mission_msg.ego_lane = mission_planner_messages::msg::DrivingCorridor(); + mission_msg.ego_lane = autoware_planning_msgs::msg::DrivingCorridor(); // add points to the ego lane centerline mission_msg.ego_lane.centerline.push_back(geometry_msgs::msg::Point()); @@ -60,7 +58,7 @@ int TestMissionToTrajectory() EXPECT_EQ(trj_msg.points.back().pose.position.y, mission_msg.ego_lane.centerline.back().y); // TEST 3: neighbor lane left - mission_msg.drivable_lanes_left.push_back(mission_planner_messages::msg::DrivingCorridor()); + mission_msg.drivable_lanes_left.push_back(autoware_planning_msgs::msg::DrivingCorridor()); mission_msg.drivable_lanes_left.back().centerline.push_back(geometry_msgs::msg::Point()); mission_msg.drivable_lanes_left.back().centerline.back().x = 0.0; mission_msg.drivable_lanes_left.back().centerline.back().y = 0.0; @@ -85,7 +83,7 @@ int TestMissionToTrajectory() mission_msg.drivable_lanes_left.back().centerline.back().y); // TEST 4: neighbor lane right - mission_msg.drivable_lanes_right.push_back(mission_planner_messages::msg::DrivingCorridor()); + mission_msg.drivable_lanes_right.push_back(autoware_planning_msgs::msg::DrivingCorridor()); mission_msg.drivable_lanes_right.back().centerline.push_back(geometry_msgs::msg::Point()); mission_msg.drivable_lanes_right.back().centerline.back().x = 1.0; mission_msg.drivable_lanes_right.back().centerline.back().y = 1.2; diff --git a/planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt b/planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt index 67a2439354f55..a23ea3c8d365a 100644 --- a/planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt +++ b/planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt @@ -22,8 +22,9 @@ ament_target_dependencies(${PROJECT_NAME} tf2 tf2_ros tf2_geometry_msgs - mission_planner_messages - db_msgs) + db_msgs + lanelet2_core + autoware_planning_msgs) # include public headers target_include_directories(${PROJECT_NAME} PUBLIC @@ -39,8 +40,9 @@ ament_export_dependencies( tf2 tf2_ros tf2_geometry_msgs - mission_planner_messages - db_msgs) + db_msgs + lanelet2_core + autoware_planning_msgs) # install library install( diff --git a/planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp b/planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp index e2c542ba79c51..1585a1073e24d 100644 --- a/planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp +++ b/planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp @@ -5,7 +5,8 @@ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" -#include "mission_planner_messages/msg/road_segments.hpp" +#include "lanelet2_core/primitives/Lanelet.h" +#include "autoware_planning_msgs/msg/road_segments.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -94,11 +95,186 @@ std::vector GetPsiForPoints(const std::vector * @brief Convert the LaneletsStamped message into a RoadSegments message. * * @param msg The message (db_msgs::msg::LaneletsStamped). - * @return mission_planner_messages::msg::RoadSegments. + * @return autoware_planning_msgs::msg::RoadSegments. */ -mission_planner_messages::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( +autoware_planning_msgs::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( const db_msgs::msg::LaneletsStamped & msg); +/** + * @brief Holds + * the origin lanelet Id, + * the predecessor lanelet Ids, + * the successor lanelet Ids, + * the neighboring lanelet Ids + * the goal information + * + */ +struct LaneletConnection +{ + int original_lanelet_id; + std::vector predecessor_lanelet_ids; + std::vector successor_lanelet_ids; + std::vector neighbor_lanelet_ids; + bool goal_information; +}; + +/** + * @brief The adjacent lane type. + * + */ +enum AdjacentLaneType { kSuccessors = 0, kPredecessors = 1 }; + +/** + * @brief Get all sequences of successor lanelets to the initial lanelet. + This function wraps GetAllLaneletSequences() + + * @param lanelet_connections Relation between individual lanelets + * (successors/predecessors/neighbors) + * @param id_initial_lanelet ID of lanelet from where neighbor search is + started + * @return Collection of sequences of all successor lanelets + */ + +std::vector> GetAllSuccessorSequences( + const std::vector & lanelet_connections, const int id_initial_lanelet); + +/** +* @brief Get all sequences of adjacent (either successors or predecessors) + lanelets to the initial lanelet. + +* @param lanelet_connections Relation between individual lanelets +* (successors/predecessors/neighbors) +* @param id_initial_lanelet ID of lanelet from where neighbor search is + started +* @param adjacent_lane_type Specifies whether predecessors or successors +should be targeted (e.g. lanelet_tools::AdjacentLaneType::kPredecessors) +* +* @return Collection of sequences of all adjacent lanelets +*/ +std::vector> GetAllLaneletSequences( + const std::vector & lanelet_connections, const int id_initial_lanelet, + const AdjacentLaneType adjacent_lane_type); + +/** +* @brief Find relevant adjacent (successors or predecessors) lanelets + (currently relevant means leading towards goal) among a set of +provided adjacent lanelets (ids_adjacent_lanelets) +* +* @param lanelet_connections Relation between individual lanelets + (successors/neighbors) +* @param ids_adjacent_lanelets IDs of all available adjacent lanelets + (either successors or predecessors) +* @param do_include_navigation_info Whether to use navigation info to +* determine relevant successors (true) or not (false); if +navigation info is not used, the ID of the first direct successors will be + returned +* @return ID of relevant successor lanelet +*/ +std::vector GetRelevantAdjacentLanelets( + const std::vector & lanelet_connections, + const std::vector ids_adjacent_lanelets, const bool do_include_navigation_info); + +/** + * @brief Get a complete lanelet ID sequence starting from an initial lanelet. + * + * @param lanelet_id_sequence_current Current lanelet ID sequence (of + * previous iteration); this is the start for the search in the current + * iteration + * @param lanelets_already_visited List of already visited lanelet IDs + * @param ids_relevant_lanelets IDs of the relevant adjacent + * (successor or predecessor) lanelets + * @param id_initial_lanelet ID of lanelet from which search was + * started initially + * @return - Vector containing IDs of a completed lanelet sequence; is empty + * when the sequence is not completed yet (i.e. there are still some unvisited adjacent + * lanelets left) + * - Flag to indicate whether outer for loop should be exited; this is necessary when no + * unvisited lanelets are left from the initial lanelet + */ +std::tuple, bool> GetCompletedLaneletSequence( + std::vector & lanelet_id_sequence_current, std::vector & lanelets_already_visited, + const std::vector ids_relevant_lanelets, const int id_initial_lanelet); + +/** + * @brief The vehicle side. + * + */ +enum VehicleSide { kLeft = 0, kRight = 1 }; + +/** + * @brief Get all neighboring lanelet IDs on one side. + * + * @param lanelet_connections Relation between individual lanelets + * (successors/neighbors) + * @param id_initial_lanelet ID of lanelet from where neighbor search is + started + * @param side Side of initial lanelet where neighboring + lanelet ID should get searched + * @return IDs of all neighboring lanelets (returns -1 if no neighbor + available) + */ +std::vector GetAllNeighboringLaneletIDs( + const std::vector & lanelet_connections, const int id_initial_lanelet, + const VehicleSide side); + +/** + * @brief Get the ID of a specified neighboring lanelet. + * + * @param lanelet_connections Relation between individual lanelets + * (successors/neighbors) + * @param id_initial_lanelet ID of lanelet from where neighbor search is + started + * @param side Side of initial lanelet where neighboring + lanelet ID should get searched + * @param do_include_navigation_info Whether to use navigation info to + * determine relevant neighbors (true) or + * not (false) + * @param recursiveness (defaults to 1) + * level of recursiveness for neighbor search + * - recursivenes=-1 means that the most outside lanes are requested + (returns initial lanelet ID if no neighbors are available -> initial lanelet + is the most outside lane) + * - recursiveness>=0 means that the direct neighbors (1), neighbors of + neighbors (2), ..., are searched (returns -1 if no lanelet is + available at the specified level of recursiveness) + * + * @return ID of neighboring lanelet (returns -1 if no neighbor available) + */ +int GetNeighboringLaneletID( + const std::vector & lanelet_connections, const int id_initial_lanelet, + const VehicleSide side, const bool do_include_navigation_info, const int recursiveness = 1); + +/** + * @brief Get all sequences of predecessor lanelets to the initial lanelet. + This function wraps GetAllLaneletSequences() + + * @param lanelet_connections Relation between individual lanelets + * (successors/predecessors/neighbors) + * @param id_initial_lanelet ID of lanelet from where neighbor search is + started + * @return Collection of sequences of all predecessor lanelets + */ +std::vector> GetAllPredecessorSequences( + const std::vector & lanelet_connections, const int id_initial_lanelet); + +/** + * @brief Finds the ID of lanelet where a given position is located in + * + * @param lanelets Collection of lanelets to search + * @param position Position of which the corresponding lanelet ID is wanted + * @return lanelet ID (returns -1 if no match) + */ +int FindOccupiedLaneletID( + const std::vector & lanelets, const lanelet::BasicPoint2d & position); + +/** + * @brief Finds the ID of the ego vehicle occupied lanelet + * + * @param lanelets Collection of lanelets to search + * @return lanelet ID (returns -1 if no match) + */ +int FindEgoOccupiedLaneletID(const std::vector & lanelets); + } // namespace lib_mission_planner #endif // LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/local_mission_planner_common/package.xml b/planning/mapless_architecture/local_mission_planner_common/package.xml index 593f2d803a001..5b49b2e575169 100644 --- a/planning/mapless_architecture/local_mission_planner_common/package.xml +++ b/planning/mapless_architecture/local_mission_planner_common/package.xml @@ -11,10 +11,11 @@ db_msgs geometry_msgs - mission_planner_messages + lanelet2_core tf2 tf2_geometry_msgs tf2_ros + autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp index 4926662c5125f..b5bc5feab3043 100644 --- a/planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp @@ -2,7 +2,8 @@ // driveblocks proprietary license #include "lib_mission_planner/helper_functions.hpp" -#include "mission_planner_messages/msg/road_segments.hpp" +#include "lanelet2_core/geometry/Lanelet.h" +#include "autoware_planning_msgs/msg/road_segments.hpp" #include "rclcpp/rclcpp.hpp" #include "db_msgs/msg/lanelets_stamped.hpp" @@ -131,11 +132,11 @@ double GetYawFromQuaternion(const double x, const double y, const double z, cons // Declare a static logger static rclcpp::Logger static_logger = rclcpp::get_logger("static_logger"); -mission_planner_messages::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( +autoware_planning_msgs::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( const db_msgs::msg::LaneletsStamped & msg) { // Initialize road segments message - mission_planner_messages::msg::RoadSegments road_segments; + autoware_planning_msgs::msg::RoadSegments road_segments; // Fill message header and pose road_segments.header = msg.header; @@ -145,7 +146,7 @@ mission_planner_messages::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( if (!msg.lanelets.empty()) { for (const db_msgs::msg::Lanelet & lanelet : msg.lanelets) { // Initialize a segment - mission_planner_messages::msg::Segment segment; + autoware_planning_msgs::msg::Segment segment; // Fill the segment with basic information segment.id = lanelet.id; @@ -176,4 +177,267 @@ mission_planner_messages::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( return road_segments; } +std::vector> GetAllSuccessorSequences( + const std::vector & lanelet_connections, const int id_initial_lanelet) +{ + AdjacentLaneType adjacent_lane_type = AdjacentLaneType::kSuccessors; + + return GetAllLaneletSequences(lanelet_connections, id_initial_lanelet, adjacent_lane_type); +} + +std::vector> GetAllLaneletSequences( + const std::vector & lanelet_connections, const int id_initial_lanelet, + const AdjacentLaneType adjacent_lane_type) +{ + // initialize helper variables + bool do_include_navigation_info = false; + + std::vector lanelets_already_visited; + std::vector lanelet_id_sequence_temp{id_initial_lanelet}; + + std::vector> lanelet_sequences; + + // loop as long as all successors of the initial lanelet have been searched + // maximum iteration depth is (number_of_lanelets - 1) * 2 + for (size_t i = 0; i < lanelet_connections.size() * 2; i++) { + // IDs which are relevant for searching adjacent lanelets (either successors + // or predecessors) + std::vector ids_adjacent_lanelets; + + if (adjacent_lane_type == AdjacentLaneType::kPredecessors) { + ids_adjacent_lanelets = + lanelet_connections[lanelet_id_sequence_temp.back()].predecessor_lanelet_ids; + } else { + ids_adjacent_lanelets = + lanelet_connections[lanelet_id_sequence_temp.back()].successor_lanelet_ids; + } + + const std::vector ids_relevant_adjacent_lanelets = GetRelevantAdjacentLanelets( + lanelet_connections, ids_adjacent_lanelets, do_include_navigation_info); + + auto [lanelet_id_sequence_completed, do_exit_outer_for_loop] = GetCompletedLaneletSequence( + lanelet_id_sequence_temp, lanelets_already_visited, ids_relevant_adjacent_lanelets, + id_initial_lanelet); + + if (do_exit_outer_for_loop) break; + + // store returned complete lanelet id sequence + if (!lanelet_id_sequence_completed.empty()) { + lanelet_sequences.push_back(lanelet_id_sequence_completed); + } + } + return lanelet_sequences; +} + +std::vector GetRelevantAdjacentLanelets( + const std::vector & lanelet_connections, + const std::vector ids_adjacent_lanelets, const bool do_include_navigation_info) +{ + std::vector ids_relevant_successors; + + // return all successors if navigation info is not relevant + if (do_include_navigation_info) { + // loop through all successors and check if they lead towards the navigation + // goal + if (ids_adjacent_lanelets.front() >= 0) { + for (std::size_t i = 0; i < ids_adjacent_lanelets.size(); i++) { + // check if successor leads to goal + if (lanelet_connections[ids_adjacent_lanelets[i]].goal_information) { + ids_relevant_successors.push_back(ids_adjacent_lanelets[i]); + } + } + } + // default return: valid successor lanelet not available + if (ids_relevant_successors.empty()) { + ids_relevant_successors.push_back(-1); + } + } else { + ids_relevant_successors = ids_adjacent_lanelets; + } + + // FIXME: avoid that list is empty -> has to be -1 if no relevant lanelet + // exists + // this fixes an issue that originates in the lanelet converter; should be + // fixed there! + if (ids_relevant_successors.empty()) { + ids_relevant_successors.push_back(-1); + } + + return ids_relevant_successors; +} + +std::tuple, bool> GetCompletedLaneletSequence( + std::vector & lanelet_id_sequence_current, std::vector & lanelets_already_visited, + const std::vector ids_relevant_lanelets, const int id_initial_lanelet) +{ + std::vector lanelet_id_sequence_completed; + bool do_exit_outer_for_loop = false; + + // check if an adjacent lanelet is even available + if (ids_relevant_lanelets.front() >= 0) { + bool has_relevant_successor = false; + + // loop though all relevant adjacent IDs and check whether or not they + // have already been visited + for (const int & successor_id : ids_relevant_lanelets) { + if ( + std::find(lanelets_already_visited.begin(), lanelets_already_visited.end(), successor_id) == + lanelets_already_visited.end()) { + // add new ID to already visited vector + lanelets_already_visited.push_back(successor_id); + // add currently visited ID to temporary adjacent lanelet sequence + lanelet_id_sequence_current.push_back(successor_id); + has_relevant_successor = true; + break; + } + } + + // if all available adjacent lanelets were already visited, go back to + // parent lanelet or exit loop + if (!has_relevant_successor) { + // pop parent lanelet except it is already the initial lanelet + if (lanelet_id_sequence_current.back() != id_initial_lanelet) { + lanelet_id_sequence_current.pop_back(); + } else { + // exit loop if initial lanelet has no relevant adjacent lanelets left + do_exit_outer_for_loop = true; + } + } + + } else { + // exit loop if initial lanelet has already no relevant adjacent lanelets + if (lanelet_id_sequence_current.back() == id_initial_lanelet) { + do_exit_outer_for_loop = true; + } else { + // store lanelet sequence when it is completed (no unvisited adjacent + // lanelets left from current lanelet) + lanelet_id_sequence_completed = lanelet_id_sequence_current; + + // go back to parent lanelet ID and continue search from there + lanelet_id_sequence_current.pop_back(); + } + } + return {lanelet_id_sequence_completed, do_exit_outer_for_loop}; +} + +std::vector GetAllNeighboringLaneletIDs( + const std::vector & lanelet_connections, const int id_initial_lanelet, + const VehicleSide side) +{ + int id_current_lanelet = id_initial_lanelet; + std::vector lanelet_id_neighbors; + + // this function is only intended to return all neighboring lanelets, not only + // the ones leading towards goal. Therefore, this flag is set false for the + // sub-function. + const bool do_include_navigation_info = false; + + // loop as long as all left or right neighbors of the initial lanelet have + // been searched + // maximum iteration depth is: number_of_lanelets + for (size_t i = 0; i < lanelet_connections.size(); i++) { + int idx_tmp = GetNeighboringLaneletID( + lanelet_connections, id_current_lanelet, side, do_include_navigation_info); + + // if ID >= 0, continue search, else break + if (idx_tmp >= 0) { + id_current_lanelet = idx_tmp; + lanelet_id_neighbors.push_back(id_current_lanelet); + } else { + // if return vector is still empty because no neighbors are available, + // return -1 + if (lanelet_id_neighbors.empty()) { + lanelet_id_neighbors.push_back(-1); + } + break; + } + } + + return lanelet_id_neighbors; +} + +int GetNeighboringLaneletID( + const std::vector & lanelet_connections, const int id_initial_lanelet, + const VehicleSide side, const bool do_include_navigation_info, const int recursiveness) +{ + // set default return id if initial lane has already no neighbors + int id_neighbor_lanelet = -1; + + // check if most outside lane is searched + // here: return current lanelet's ID when no neighbors are available + if (recursiveness == -1) { + id_neighbor_lanelet = id_initial_lanelet; + } + + // get ID of current lanelet's neighboring lanelet + int id_neighbor_lanelet_tmp = lanelet_connections[id_initial_lanelet].neighbor_lanelet_ids[side]; + + // init counter to track current level of recursiveness in loop + int recursiveness_counter = 0; + + // loop as long as all neighbors of the initial lanelet have been searched + // maximum iteration depth: number_of_lanelets + for (size_t i = 0; i < lanelet_connections.size(); i++) { + // break while loop if desired recursiveness is reached + if (recursiveness != -1 && recursiveness_counter >= recursiveness) break; + + // check if neighbor is available + if (id_neighbor_lanelet_tmp >= 0) { + // break if navigation info is considered AND the lanelet does not lead + // towards goal + if ( + do_include_navigation_info && + !lanelet_connections[id_neighbor_lanelet_tmp].goal_information) { + break; + } + + // store current neighbor lanelet ID + id_neighbor_lanelet = id_neighbor_lanelet_tmp; + // query neighbor of neighbor and use as new start lanelet + id_neighbor_lanelet_tmp = + lanelet_connections[id_neighbor_lanelet_tmp].neighbor_lanelet_ids[side]; + + } else { + // return -1 if lanelet ID at specific recursiveness is requested + if (recursiveness >= 0) { + id_neighbor_lanelet = -1; + } + break; + } + recursiveness_counter++; + } + return id_neighbor_lanelet; +} + +std::vector> GetAllPredecessorSequences( + const std::vector & lanelet_connections, const int id_initial_lanelet) +{ + AdjacentLaneType adjacent_lane_type = AdjacentLaneType::kPredecessors; + + return GetAllLaneletSequences(lanelet_connections, id_initial_lanelet, adjacent_lane_type); +} + +int FindOccupiedLaneletID( + const std::vector & lanelets, const lanelet::BasicPoint2d & position) +{ + int id_occupied_lanelet = -1; + + // check if position is within one of the available lanelet + for (size_t i = 0; i < lanelets.size(); i++) { + if (lanelet::geometry::inside(lanelets[i], position)) { + id_occupied_lanelet = i; + break; + } + } + + return id_occupied_lanelet; +} + +int FindEgoOccupiedLaneletID(const std::vector & lanelets) +{ + const lanelet::BasicPoint2d position_ego = lanelet::BasicPoint2d(0.0, 0.0); + + return FindOccupiedLaneletID(lanelets, position_ego); +} + } // namespace lib_mission_planner From a12fcd79354a0fdd1351b1c04500adb83790a36e Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 20 Jun 2024 13:55:12 +0200 Subject: [PATCH 06/24] Rename packages Signed-off-by: Simon Eisenmann --- .../mapless_architecture/autoware_hmi/CMakeLists.txt | 2 +- .../include/{ => autoware/hmi}/hmi_node.hpp | 0 planning/mapless_architecture/autoware_hmi/package.xml | 2 +- .../mapless_architecture/autoware_hmi/src/hmi_main.cpp | 2 +- .../mapless_architecture/autoware_hmi/src/hmi_node.cpp | 2 +- .../autoware_local_map_provider/CMakeLists.txt | 2 +- .../local_map_provider}/local_map_provider_node.hpp | 0 .../autoware_local_map_provider/package.xml | 2 +- .../src/local_map_provider_main.cpp | 2 +- .../src/local_map_provider_node.cpp | 2 +- .../autoware_local_mission_planner/CMakeLists.txt | 2 +- .../local_mission_planner}/mission_planner_node.hpp | 2 +- .../autoware_local_mission_planner/package.xml | 4 ++-- .../src/mission_planner_main.cpp | 2 +- .../src/mission_planner_node.cpp | 4 ++-- .../test/src/test_mission_planner_core.cpp | 4 ++-- .../CMakeLists.txt | 4 ++-- .../Readme.md | 0 .../local_mission_planner_common}/helper_functions.hpp | 0 .../package.xml | 2 +- .../src/helper_functions.cpp | 2 +- .../test/gtest_main.cpp | 1 - .../autoware_local_road_provider/CMakeLists.txt | 2 +- .../local_road_provider}/local_road_provider_node.hpp | 0 .../autoware_local_road_provider/package.xml | 4 ++-- .../src/local_road_provider_main.cpp | 2 +- .../src/local_road_provider_node.cpp | 4 ++-- .../autoware_mission_lane_converter/CMakeLists.txt | 2 +- .../mission_lane_converter_node.hpp | 0 .../autoware_mission_lane_converter/package.xml | 4 ++-- .../src/main_mission_lane_converter.cpp | 2 +- .../src/mission_lane_converter_node.cpp | 4 ++-- .../test/src/test_mission_planner_converter.cpp | 2 +- .../test/include/test_helper_functions.hpp | 10 ---------- .../test/src/test_helper_functions.cpp | 8 -------- 35 files changed, 34 insertions(+), 53 deletions(-) rename planning/mapless_architecture/autoware_hmi/include/{ => autoware/hmi}/hmi_node.hpp (100%) rename planning/mapless_architecture/autoware_local_map_provider/include/{ => autoware/local_map_provider}/local_map_provider_node.hpp (100%) rename planning/mapless_architecture/autoware_local_mission_planner/include/{ => autoware/local_mission_planner}/mission_planner_node.hpp (99%) rename planning/mapless_architecture/{local_mission_planner_common => autoware_local_mission_planner_common}/CMakeLists.txt (95%) rename planning/mapless_architecture/{local_mission_planner_common => autoware_local_mission_planner_common}/Readme.md (100%) rename planning/mapless_architecture/{local_mission_planner_common/include/lib_mission_planner => autoware_local_mission_planner_common/include/autoware/local_mission_planner_common}/helper_functions.hpp (100%) rename planning/mapless_architecture/{local_mission_planner_common => autoware_local_mission_planner_common}/package.xml (94%) rename planning/mapless_architecture/{local_mission_planner_common => autoware_local_mission_planner_common}/src/helper_functions.cpp (99%) rename planning/mapless_architecture/{local_mission_planner_common => autoware_local_mission_planner_common}/test/gtest_main.cpp (86%) rename planning/mapless_architecture/autoware_local_road_provider/include/{ => autoware/local_road_provider}/local_road_provider_node.hpp (100%) rename planning/mapless_architecture/autoware_mission_lane_converter/include/{ => autoware/mission_lane_converter}/mission_lane_converter_node.hpp (100%) delete mode 100644 planning/mapless_architecture/local_mission_planner_common/test/include/test_helper_functions.hpp delete mode 100644 planning/mapless_architecture/local_mission_planner_common/test/src/test_helper_functions.cpp diff --git a/planning/mapless_architecture/autoware_hmi/CMakeLists.txt b/planning/mapless_architecture/autoware_hmi/CMakeLists.txt index e4a7957f6f73a..1bc934c5b3360 100644 --- a/planning/mapless_architecture/autoware_hmi/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_hmi/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(hmi) +project(autoware_hmi) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp similarity index 100% rename from planning/mapless_architecture/autoware_hmi/include/hmi_node.hpp rename to planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp diff --git a/planning/mapless_architecture/autoware_hmi/package.xml b/planning/mapless_architecture/autoware_hmi/package.xml index fc306c7dfc175..fd8e08ceea8c0 100644 --- a/planning/mapless_architecture/autoware_hmi/package.xml +++ b/planning/mapless_architecture/autoware_hmi/package.xml @@ -1,7 +1,7 @@ - hmi + autoware_hmi 0.0.1 HMI driveblocks diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp index bf2045fdbd9cd..47adb91a705a3 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp @@ -1,7 +1,7 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "hmi_node.hpp" +#include "autoware/hmi/hmi_node.hpp" #include diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index 0d8e9079508c2..2324ff4e39af3 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -1,7 +1,7 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "hmi_node.hpp" +#include "autoware/hmi/hmi_node.hpp" #include "autoware_planning_msgs/msg/mission.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt b/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt index 8a2de3f45787f..2321e54572bc1 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(local_map_provider) +project(autoware_local_map_provider) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp similarity index 100% rename from planning/mapless_architecture/autoware_local_map_provider/include/local_map_provider_node.hpp rename to planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp diff --git a/planning/mapless_architecture/autoware_local_map_provider/package.xml b/planning/mapless_architecture/autoware_local_map_provider/package.xml index f2464d374d5d4..f8a00b0501226 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_map_provider/package.xml @@ -1,7 +1,7 @@ - local_map_provider + autoware_local_map_provider 0.0.1 local_map_provider driveblocks diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp index 99ad196171522..63a504e1c841d 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp @@ -1,7 +1,7 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "local_map_provider_node.hpp" +#include "autoware/local_map_provider/local_map_provider_node.hpp" #include diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp index 8f296ddc36dd4..6471a2cf33f25 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -1,7 +1,7 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "local_map_provider_node.hpp" +#include "autoware/local_map_provider/local_map_provider_node.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt index 0558617dcb323..d4f25246d0f28 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(mission_planner) +project(autoware_local_mission_planner) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp similarity index 99% rename from planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp rename to planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 644aa23eac40e..68c6442968734 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -4,7 +4,7 @@ #define MISSION_PLANNER_NODE_HPP_ #include "lanelet2_core/geometry/LineString.h" -#include "lib_mission_planner/helper_functions.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "autoware_planning_msgs/msg/driving_corridor.hpp" #include "autoware_planning_msgs/msg/local_map.hpp" #include "autoware_planning_msgs/msg/mission.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/autoware_local_mission_planner/package.xml index 1301143db8217..a5eb4f05b4cab 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner/package.xml @@ -1,7 +1,7 @@ - mission_planner + autoware_local_mission_planner 0.0.1 Mission Planner driveblocks @@ -15,7 +15,7 @@ db_msgs geometry_msgs lanelet2_core - lib_mission_planner + autoware_local_mission_planner_common rclcpp tf2 tf2_geometry_msgs diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp index 5a770e5a29054..244afb124d4eb 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp @@ -1,6 +1,6 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "mission_planner_node.hpp" +#include "autoware/local_mission_planner/mission_planner_node.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 4c10d995e42e2..485c2048380f9 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -1,11 +1,11 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "mission_planner_node.hpp" +#include "autoware/local_mission_planner/mission_planner_node.hpp" #include "lanelet2_core/LaneletMap.h" #include "lanelet2_core/geometry/Lanelet.h" #include "lanelet2_core/geometry/LineString.h" -#include "lib_mission_planner/helper_functions.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "autoware_planning_msgs/msg/driving_corridor.hpp" #include "autoware_planning_msgs/msg/mission.hpp" #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index 69463cb49abc4..3e65d63af55f7 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -2,8 +2,8 @@ // driveblocks proprietary license #include "gtest/gtest.h" -#include "lib_mission_planner/helper_functions.hpp" -#include "mission_planner_node.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" +#include "autoware/local_mission_planner/mission_planner_node.hpp" #include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt similarity index 95% rename from planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt rename to planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt index a23ea3c8d365a..38096de1dc697 100644 --- a/planning/mapless_architecture/local_mission_planner_common/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(lib_mission_planner) +project(autoware_local_mission_planner_common) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -71,7 +71,7 @@ if(BUILD_TESTING) # MAIN test/gtest_main.cpp # - test/src/test_helper_functions.cpp src/helper_functions.cpp) + src/helper_functions.cpp) ament_lint_auto_find_test_dependencies() target_include_directories(${PROJECT_NAME}_tests PRIVATE test/include) diff --git a/planning/mapless_architecture/local_mission_planner_common/Readme.md b/planning/mapless_architecture/autoware_local_mission_planner_common/Readme.md similarity index 100% rename from planning/mapless_architecture/local_mission_planner_common/Readme.md rename to planning/mapless_architecture/autoware_local_mission_planner_common/Readme.md diff --git a/planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp similarity index 100% rename from planning/mapless_architecture/local_mission_planner_common/include/lib_mission_planner/helper_functions.hpp rename to planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp diff --git a/planning/mapless_architecture/local_mission_planner_common/package.xml b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml similarity index 94% rename from planning/mapless_architecture/local_mission_planner_common/package.xml rename to planning/mapless_architecture/autoware_local_mission_planner_common/package.xml index 5b49b2e575169..6b95f38355bdb 100644 --- a/planning/mapless_architecture/local_mission_planner_common/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml @@ -1,7 +1,7 @@ - lib_mission_planner + autoware_local_mission_planner_common 0.0.0 Library containing helper functions for the different nodes. driveblocks diff --git a/planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp similarity index 99% rename from planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp rename to planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index b5bc5feab3043..c721df3863637 100644 --- a/planning/mapless_architecture/local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -1,6 +1,6 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "lib_mission_planner/helper_functions.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "lanelet2_core/geometry/Lanelet.h" #include "autoware_planning_msgs/msg/road_segments.hpp" diff --git a/planning/mapless_architecture/local_mission_planner_common/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp similarity index 86% rename from planning/mapless_architecture/local_mission_planner_common/test/gtest_main.cpp rename to planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp index 27aada7ff9823..943b5400ca2c7 100644 --- a/planning/mapless_architecture/local_mission_planner_common/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp @@ -1,7 +1,6 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license #include "gtest/gtest.h" -#include "test_helper_functions.hpp" /** * Test helper functions diff --git a/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt b/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt index 77f141fcc9d44..a5bd0b689d0d6 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(local_road_provider) +project(autoware_local_road_provider) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp similarity index 100% rename from planning/mapless_architecture/autoware_local_road_provider/include/local_road_provider_node.hpp rename to planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp diff --git a/planning/mapless_architecture/autoware_local_road_provider/package.xml b/planning/mapless_architecture/autoware_local_road_provider/package.xml index 883ce76a9ddb5..bf975d375e90c 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_road_provider/package.xml @@ -1,7 +1,7 @@ - local_road_provider + autoware_local_road_provider 0.0.1 local_road_provider driveblocks @@ -12,7 +12,7 @@ ros2launch db_msgs - lib_mission_planner + autoware_local_mission_planner_common rclcpp autoware_planning_msgs diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp index 00ddfe6eb9584..587a3d4b282b3 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp @@ -1,7 +1,7 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "local_road_provider_node.hpp" +#include "autoware/local_road_provider/local_road_provider_node.hpp" #include diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp index 9870d85ab9cd6..09045726ff0e4 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp @@ -1,9 +1,9 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "local_road_provider_node.hpp" +#include "autoware/local_road_provider/local_road_provider_node.hpp" -#include "lib_mission_planner/helper_functions.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "rclcpp/rclcpp.hpp" using std::placeholders::_1; diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt b/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt index c6d4e8f629e51..b8be6a1efd25b 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(mission_lane_converter) +project(autoware_mission_lane_converter) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp similarity index 100% rename from planning/mapless_architecture/autoware_mission_lane_converter/include/mission_lane_converter_node.hpp rename to planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml index b4b872356dffd..cb7befb96da83 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml +++ b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml @@ -1,7 +1,7 @@ - mission_lane_converter + autoware_mission_lane_converter 0.0.1 Mission Lane Converter driveblocks @@ -13,7 +13,7 @@ autoware_auto_planning_msgs geometry_msgs - lib_mission_planner + autoware_local_mission_planner_common rclcpp tf2_geometry_msgs visualization_msgs diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp index c3a4ce80602d7..e7183fe0abcba 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp @@ -1,6 +1,6 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "mission_lane_converter_node.hpp" +#include "autoware/mission_lane_converter/mission_lane_converter_node.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 25a4800aef43c..15e1000913780 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -1,9 +1,9 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "mission_lane_converter_node.hpp" +#include "autoware/mission_lane_converter/mission_lane_converter_node.hpp" -#include "lib_mission_planner/helper_functions.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp index 0b8a9efea8852..163bc399e0d3a 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -2,7 +2,7 @@ // driveblocks proprietary license #include "gtest/gtest.h" -#include "mission_lane_converter_node.hpp" +#include "autoware/mission_lane_converter/mission_lane_converter_node.hpp" #include "geometry_msgs/msg/point.hpp" diff --git a/planning/mapless_architecture/local_mission_planner_common/test/include/test_helper_functions.hpp b/planning/mapless_architecture/local_mission_planner_common/test/include/test_helper_functions.hpp deleted file mode 100644 index 6ad8f869ae752..0000000000000 --- a/planning/mapless_architecture/local_mission_planner_common/test/include/test_helper_functions.hpp +++ /dev/null @@ -1,10 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license -#ifndef TEST_HELPER_FUNCTIONS_HPP_ -#define TEST_HELPER_FUNCTIONS_HPP_ - -#include "lib_mission_planner/helper_functions.hpp" - -// TODO(simon.eisenmann@driveblocks.ai): Add test functions - -#endif // TEST_HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/local_mission_planner_common/test/src/test_helper_functions.cpp b/planning/mapless_architecture/local_mission_planner_common/test/src/test_helper_functions.cpp deleted file mode 100644 index bcbd5c7faf704..0000000000000 --- a/planning/mapless_architecture/local_mission_planner_common/test/src/test_helper_functions.cpp +++ /dev/null @@ -1,8 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license -#include "test_helper_functions.hpp" - -#include "gtest/gtest.h" -#include "lib_mission_planner/helper_functions.hpp" - -// TODO(simon.eisenmann@driveblocks.ai): Add test functions From b589b532503b2b708e6c041c211fff34c46985d3 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 20 Jun 2024 14:37:56 +0200 Subject: [PATCH 07/24] Add namespace Signed-off-by: Simon Eisenmann --- .../autoware_hmi/include/autoware/hmi/hmi_node.hpp | 4 ++++ planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp | 2 +- planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp | 3 +++ .../autoware/local_map_provider/local_map_provider_node.hpp | 3 +++ .../src/local_map_provider_main.cpp | 2 +- .../src/local_map_provider_node.cpp | 2 ++ .../autoware/local_mission_planner/mission_planner_node.hpp | 3 +++ .../src/mission_planner_main.cpp | 2 +- .../src/mission_planner_node.cpp | 3 +++ .../autoware_local_mission_planner/test/gtest_main.cpp | 4 ++++ .../test/include/test_mission_planner_core.hpp | 5 +++++ .../test/src/test_mission_planner_core.cpp | 4 ++++ .../local_mission_planner_common/helper_functions.hpp | 3 +++ .../src/helper_functions.cpp | 4 ++++ .../local_road_provider/local_road_provider_node.hpp | 4 ++++ .../src/local_road_provider_main.cpp | 2 +- .../src/local_road_provider_node.cpp | 3 +++ .../mission_lane_converter/mission_lane_converter_node.hpp | 4 ++++ .../src/main_mission_lane_converter.cpp | 2 +- .../src/mission_lane_converter_node.cpp | 3 +++ .../autoware_mission_lane_converter/test/gtest_main.cpp | 5 +++++ .../test/include/test_mission_planner_converter.hpp | 5 +++++ .../test/src/test_mission_planner_converter.cpp | 4 ++++ 23 files changed, 71 insertions(+), 5 deletions(-) diff --git a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp index f17bc6c47df56..66ca025029d63 100644 --- a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp +++ b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp @@ -9,6 +9,9 @@ #include #include +namespace autoware::mapless_architecture +{ + /** * Node for HMI. */ @@ -51,5 +54,6 @@ class HMINode : public rclcpp::Node rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; }; +} #endif // HMI_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp index 47adb91a705a3..9dbcada942bf1 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp @@ -12,7 +12,7 @@ int main(int argc, char * argv[]) RCLCPP_INFO(rclcpp::get_logger("hmi"), "Launching HMI node..."); rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index 2324ff4e39af3..c8df0cbc7674a 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -6,6 +6,8 @@ #include "autoware_planning_msgs/msg/mission.hpp" #include "rclcpp/rclcpp.hpp" +namespace autoware::mapless_architecture +{ using std::placeholders::_1; /** @@ -82,3 +84,4 @@ void HMINode::PublishMission_(std::string mission) mission_publisher_->publish(missionMessage); } +} diff --git a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp index 915da1aa1edee..6d6813db3ad6f 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp @@ -7,6 +7,8 @@ #include "autoware_planning_msgs/msg/road_segments.hpp" #include "rclcpp/rclcpp.hpp" +namespace autoware::mapless_architecture{ + /** * Node for the Local Map Provider. */ @@ -42,5 +44,6 @@ class LocalMapProviderNode : public rclcpp::Node rclcpp::Subscription::SharedPtr road_subscriber_; }; +} #endif // LOCAL_MAP_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp index 63a504e1c841d..4ab340c49d077 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp @@ -12,7 +12,7 @@ int main(int argc, char * argv[]) RCLCPP_INFO(rclcpp::get_logger("local_map_provider"), "Launching Local Map Provider node..."); rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp index 6471a2cf33f25..996d60dff0d68 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -5,6 +5,7 @@ #include "rclcpp/rclcpp.hpp" +namespace autoware::mapless_architecture{ using std::placeholders::_1; LocalMapProviderNode::LocalMapProviderNode() : Node("local_map_provider_node") @@ -37,3 +38,4 @@ void LocalMapProviderNode::CallbackRoadSegmentsMessages_( map_publisher_->publish( local_map); // Outlook: Add global map, sign detection etc. to the message } +} diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 68c6442968734..059c8e1b020fb 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -27,6 +27,8 @@ #include #include +namespace autoware::mapless_architecture +{ using namespace lib_mission_planner; // Create Direction data type @@ -336,5 +338,6 @@ class MissionPlannerNode : public rclcpp::Node // Unique ID for each marker int centerline_marker_id_ = 0; }; +} #endif // MISSION_PLANNER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp index 244afb124d4eb..9b1f612c85de9 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp @@ -10,7 +10,7 @@ int main(int argc, char * argv[]) RCLCPP_INFO(rclcpp::get_logger("mission_planner"), "Launching mission planner node..."); rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 485c2048380f9..27cf26172c6c0 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -27,6 +27,8 @@ #include #include +namespace autoware::mapless_architecture +{ using std::placeholders::_1; using namespace lib_mission_planner; @@ -1116,3 +1118,4 @@ void MissionPlannerNode::CalculatePredecessors(std::vector & } } } +} diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp index 3eb86a362bb0d..7d2e33a8b772f 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp @@ -5,6 +5,9 @@ #include "include/test_mission_planner_core.hpp" #include "rclcpp/rclcpp.hpp" +namespace autoware::mapless_architecture +{ + TEST(MissionPlannerCore, CalculateDistanceBetweenPointAndLineString) { EXPECT_EQ(TestCalculateDistanceBetweenPointAndLineString(), 0); @@ -52,3 +55,4 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); rclcpp::shutdown(); } +} diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp index d5d8f0041bf75..ccccc107ced6a 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp @@ -5,6 +5,9 @@ #include "db_msgs/msg/lanelets_stamped.hpp" +namespace autoware::mapless_architecture +{ + /** * @brief Test distance between point and LineString calculation. * @@ -72,4 +75,6 @@ int TestCreateMarkerArray(); */ int TestCreateDrivingCorridor(); +} + #endif // TEST_MISSION_PLANNER_CORE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index 3e65d63af55f7..f4284e0614eca 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -11,6 +11,9 @@ #include #include +namespace autoware::mapless_architecture +{ + db_msgs::msg::LaneletsStamped CreateLanelets() { // Local variables @@ -662,3 +665,4 @@ int TestCreateDrivingCorridor() return 0; } +} diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index 1585a1073e24d..7d663bb70c485 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -16,6 +16,8 @@ #include +namespace autoware::mapless_architecture +{ namespace lib_mission_planner { @@ -276,5 +278,6 @@ int FindOccupiedLaneletID( int FindEgoOccupiedLaneletID(const std::vector & lanelets); } // namespace lib_mission_planner +} #endif // LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index c721df3863637..761a62d956b48 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -8,6 +8,8 @@ #include "db_msgs/msg/lanelets_stamped.hpp" +namespace autoware::mapless_architecture +{ namespace lib_mission_planner { @@ -441,3 +443,5 @@ int FindEgoOccupiedLaneletID(const std::vector & lanelets) } } // namespace lib_mission_planner + +} diff --git a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp index 786a1fc3f0b4c..9d92ff945cc0b 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp @@ -8,6 +8,9 @@ #include "db_msgs/msg/lanelets_stamped.hpp" +namespace autoware::mapless_architecture +{ + /** * Node for the Local Road Provider. */ @@ -43,5 +46,6 @@ class LocalRoadProviderNode : public rclcpp::Node rclcpp::Subscription::SharedPtr lanelets_subscriber_; }; +} #endif // LOCAL_ROAD_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp index 587a3d4b282b3..7f47117d1a809 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp @@ -12,7 +12,7 @@ int main(int argc, char * argv[]) RCLCPP_INFO(rclcpp::get_logger("local_road_provider"), "Launching Local Road Provider node..."); rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp index 09045726ff0e4..7d25e0d2daf1a 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp @@ -6,6 +6,8 @@ #include "autoware/local_mission_planner_common/helper_functions.hpp" #include "rclcpp/rclcpp.hpp" +namespace autoware::mapless_architecture +{ using std::placeholders::_1; LocalRoadProviderNode::LocalRoadProviderNode() : Node("local_road_provider_node") @@ -34,3 +36,4 @@ void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::Lanele // Publish the RoadSegments message road_publisher_->publish(road_segments); } +} diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp index 3a8d07ff86afe..200ad281bb5a5 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -16,6 +16,9 @@ #include #include +namespace autoware::mapless_architecture +{ + /** * Node to convert the mission lane to an autoware trajectory type. */ @@ -175,5 +178,6 @@ class MissionLaneConverterNode : public rclcpp::Node // ros parameters float target_speed_; }; +} #endif // MISSION_LANE_CONVERTER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp index e7183fe0abcba..da4616e3df094 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp @@ -12,7 +12,7 @@ int main(int argc, char * argv[]) "Launching mission lane converter node..."); rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 15e1000913780..28dc1be16d2ca 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -16,6 +16,8 @@ #include #include +namespace autoware::mapless_architecture +{ using std::placeholders::_1; using namespace lib_mission_planner; @@ -609,3 +611,4 @@ visualization_msgs::msg::Marker MissionLaneConverterNode::GetGlobalTrjVisualizat return trj_vis_global; } +} diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp index f00c8d543b7dc..b18823e3cf351 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp @@ -4,6 +4,9 @@ #include "include/test_mission_planner_converter.hpp" #include "rclcpp/rclcpp.hpp" +namespace autoware::mapless_architecture +{ + TEST(MissionConverter, MissionToTrajectory) { EXPECT_EQ(TestMissionToTrajectory(), 0); @@ -16,3 +19,5 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); rclcpp::shutdown(); } + +} diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp index ae2ddd52ff89f..5d427c217a3d6 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp @@ -4,6 +4,11 @@ #ifndef TEST_MISSION_PLANNER_CONVERTER_HPP_ #define TEST_MISSION_PLANNER_CONVERTER_HPP_ +namespace autoware::mapless_architecture +{ + int TestMissionToTrajectory(); +} + #endif // TEST_MISSION_PLANNER_CONVERTER_HPP_ diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp index 163bc399e0d3a..75a7b56419958 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -6,6 +6,9 @@ #include "geometry_msgs/msg/point.hpp" +namespace autoware::mapless_architecture +{ + int TestMissionToTrajectory() { MissionLaneConverterNode mission_converter = MissionLaneConverterNode(); @@ -109,3 +112,4 @@ int TestMissionToTrajectory() return 0; } +} From f94bdc9433f5d8f19d1febd264d6a752e2bf4508 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 20 Jun 2024 12:46:53 +0000 Subject: [PATCH 08/24] Run pre-commit Signed-off-by: Simon Eisenmann --- .../include/autoware/hmi/hmi_node.hpp | 11 ++++++----- .../autoware_hmi/package.xml | 2 +- .../autoware_hmi/src/hmi_node.cpp | 5 +++-- .../local_map_provider_node.hpp | 14 ++++++++------ .../autoware_local_map_provider/package.xml | 2 +- .../src/local_map_provider_node.cpp | 5 +++-- .../mission_planner_node.hpp | 18 +++++++++--------- .../autoware_local_mission_planner/package.xml | 4 ++-- .../param/mission_planner_default.yaml | 9 +++++---- .../src/mission_planner_node.cpp | 11 +++++------ .../test/gtest_main.cpp | 2 +- .../test/include/test_mission_planner_core.hpp | 2 +- .../test/src/test_mission_planner_core.cpp | 6 +++--- .../helper_functions.hpp | 10 +++++----- .../package.xml | 2 +- .../src/helper_functions.cpp | 4 ++-- .../local_road_provider_node.hpp | 10 +++++----- .../autoware_local_road_provider/package.xml | 4 ++-- .../src/local_road_provider_node.cpp | 2 +- .../autoware_mission_lane_converter/Readme.md | 3 ++- .../mission_lane_converter_node.hpp | 10 +++++----- .../package.xml | 4 ++-- .../param/mission_lane_converter_default.yaml | 2 +- .../src/mission_lane_converter_node.cpp | 2 +- .../test/gtest_main.cpp | 2 +- .../src/test_mission_planner_converter.cpp | 4 ++-- 26 files changed, 78 insertions(+), 72 deletions(-) diff --git a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp index 66ca025029d63..82e97d80cf42a 100644 --- a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp +++ b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp @@ -1,11 +1,12 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#ifndef HMI_NODE_HPP_ -#define HMI_NODE_HPP_ +#ifndef AUTOWARE__HMI__HMI_NODE_HPP_ +#define AUTOWARE__HMI__HMI_NODE_HPP_ -#include "autoware_planning_msgs/msg/mission.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_planning_msgs/msg/mission.hpp" + #include #include @@ -54,6 +55,6 @@ class HMINode : public rclcpp::Node rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; }; -} +} // namespace autoware::mapless_architecture -#endif // HMI_NODE_HPP_ +#endif // AUTOWARE__HMI__HMI_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_hmi/package.xml b/planning/mapless_architecture/autoware_hmi/package.xml index fd8e08ceea8c0..888cfa42bab6f 100644 --- a/planning/mapless_architecture/autoware_hmi/package.xml +++ b/planning/mapless_architecture/autoware_hmi/package.xml @@ -11,8 +11,8 @@ ros2launch - rclcpp autoware_planning_msgs + rclcpp ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index c8df0cbc7674a..33f68fc58734d 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -3,9 +3,10 @@ #include "autoware/hmi/hmi_node.hpp" -#include "autoware_planning_msgs/msg/mission.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_planning_msgs/msg/mission.hpp" + namespace autoware::mapless_architecture { using std::placeholders::_1; @@ -84,4 +85,4 @@ void HMINode::PublishMission_(std::string mission) mission_publisher_->publish(missionMessage); } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp index 6d6813db3ad6f..c684da39e79ac 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp @@ -1,13 +1,15 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#ifndef LOCAL_MAP_PROVIDER_NODE_HPP_ -#define LOCAL_MAP_PROVIDER_NODE_HPP_ +#ifndef AUTOWARE__LOCAL_MAP_PROVIDER__LOCAL_MAP_PROVIDER_NODE_HPP_ +#define AUTOWARE__LOCAL_MAP_PROVIDER__LOCAL_MAP_PROVIDER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/local_map.hpp" #include "autoware_planning_msgs/msg/road_segments.hpp" -#include "rclcpp/rclcpp.hpp" -namespace autoware::mapless_architecture{ +namespace autoware::mapless_architecture +{ /** * Node for the Local Map Provider. @@ -44,6 +46,6 @@ class LocalMapProviderNode : public rclcpp::Node rclcpp::Subscription::SharedPtr road_subscriber_; }; -} +} // namespace autoware::mapless_architecture -#endif // LOCAL_MAP_PROVIDER_NODE_HPP_ +#endif // AUTOWARE__LOCAL_MAP_PROVIDER__LOCAL_MAP_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_map_provider/package.xml b/planning/mapless_architecture/autoware_local_map_provider/package.xml index f8a00b0501226..bee517a0f6752 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_map_provider/package.xml @@ -11,10 +11,10 @@ ros2launch + autoware_planning_msgs db_msgs lib_mission_planner rclcpp - autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp index 996d60dff0d68..27e15fed30e00 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -5,7 +5,8 @@ #include "rclcpp/rclcpp.hpp" -namespace autoware::mapless_architecture{ +namespace autoware::mapless_architecture +{ using std::placeholders::_1; LocalMapProviderNode::LocalMapProviderNode() : Node("local_map_provider_node") @@ -38,4 +39,4 @@ void LocalMapProviderNode::CallbackRoadSegmentsMessages_( map_publisher_->publish( local_map); // Outlook: Add global map, sign detection etc. to the message } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 059c8e1b020fb..54e3b3a1160da 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -1,19 +1,19 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#ifndef MISSION_PLANNER_NODE_HPP_ -#define MISSION_PLANNER_NODE_HPP_ +#ifndef AUTOWARE__LOCAL_MISSION_PLANNER__MISSION_PLANNER_NODE_HPP_ +#define AUTOWARE__LOCAL_MISSION_PLANNER__MISSION_PLANNER_NODE_HPP_ -#include "lanelet2_core/geometry/LineString.h" #include "autoware/local_mission_planner_common/helper_functions.hpp" +#include "lanelet2_core/geometry/LineString.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + #include "autoware_planning_msgs/msg/driving_corridor.hpp" #include "autoware_planning_msgs/msg/local_map.hpp" #include "autoware_planning_msgs/msg/mission.hpp" #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" #include "autoware_planning_msgs/msg/visualization_distance.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" - #include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -338,6 +338,6 @@ class MissionPlannerNode : public rclcpp::Node // Unique ID for each marker int centerline_marker_id_ = 0; }; -} +} // namespace autoware::mapless_architecture -#endif // MISSION_PLANNER_NODE_HPP_ +#endif // AUTOWARE__LOCAL_MISSION_PLANNER__MISSION_PLANNER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/autoware_local_mission_planner/package.xml index a5eb4f05b4cab..3ba867af04bd6 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner/package.xml @@ -11,17 +11,17 @@ ros2launch + autoware_local_mission_planner_common + autoware_planning_msgs builtin_interfaces db_msgs geometry_msgs lanelet2_core - autoware_local_mission_planner_common rclcpp tf2 tf2_geometry_msgs tf2_ros visualization_msgs - autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml index b525b14d52bc9..27ee3f4e89d24 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml +++ b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml @@ -1,7 +1,8 @@ /mission_planner: mission_planner: ros__parameters: - distance_to_centerline_threshold: 0.25 # [m] threshold to determine if lane change mission was successful - # if ego is in proximity to the goal centerline - projection_distance_on_goallane: 20.0 # [m] projection distance of goal point of a mission - retrigger_attempts_max: 10 # Number of attempts for triggering a lane change + distance_to_centerline_threshold: + 0.25 # [m] threshold to determine if lane change mission was successful + # if ego is in proximity to the goal centerline + projection_distance_on_goallane: 20.0 # [m] projection distance of goal point of a mission + retrigger_attempts_max: 10 # Number of attempts for triggering a lane change diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 27cf26172c6c0..6040359cd5999 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -2,15 +2,15 @@ // driveblocks proprietary license #include "autoware/local_mission_planner/mission_planner_node.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "lanelet2_core/LaneletMap.h" #include "lanelet2_core/geometry/Lanelet.h" #include "lanelet2_core/geometry/LineString.h" -#include "autoware/local_mission_planner_common/helper_functions.hpp" +#include "rclcpp/rclcpp.hpp" + #include "autoware_planning_msgs/msg/driving_corridor.hpp" #include "autoware_planning_msgs/msg/mission.hpp" #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" -#include "rclcpp/rclcpp.hpp" - #include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -455,8 +455,7 @@ lanelet::BasicPoint2d MissionPlannerNode::RecenterGoalPoint( return projected_goal_point; } -void MissionPlannerNode::CallbackMissionMessages_( - const autoware_planning_msgs::msg::Mission & msg) +void MissionPlannerNode::CallbackMissionMessages_(const autoware_planning_msgs::msg::Mission & msg) { // Initialize variables lane_change_trigger_success_ = false; @@ -1118,4 +1117,4 @@ void MissionPlannerNode::CalculatePredecessors(std::vector & } } } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp index 7d2e33a8b772f..1158afd039a38 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp @@ -55,4 +55,4 @@ int main(int argc, char ** argv) return RUN_ALL_TESTS(); rclcpp::shutdown(); } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp index ccccc107ced6a..f60e324a3eea2 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp @@ -75,6 +75,6 @@ int TestCreateMarkerArray(); */ int TestCreateDrivingCorridor(); -} +} // namespace autoware::mapless_architecture #endif // TEST_MISSION_PLANNER_CORE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index f4284e0614eca..c83408737130e 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -1,9 +1,9 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "gtest/gtest.h" -#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "autoware/local_mission_planner/mission_planner_node.hpp" +#include "autoware/local_mission_planner_common/helper_functions.hpp" +#include "gtest/gtest.h" #include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -665,4 +665,4 @@ int TestCreateDrivingCorridor() return 0; } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index 7d663bb70c485..4151dae5d3e49 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -1,15 +1,15 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#ifndef LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ -#define LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ +#ifndef AUTOWARE__LOCAL_MISSION_PLANNER_COMMON__HELPER_FUNCTIONS_HPP_ +#define AUTOWARE__LOCAL_MISSION_PLANNER_COMMON__HELPER_FUNCTIONS_HPP_ #include "eigen3/Eigen/Core" #include "eigen3/Eigen/Geometry" #include "lanelet2_core/primitives/Lanelet.h" -#include "autoware_planning_msgs/msg/road_segments.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "autoware_planning_msgs/msg/road_segments.hpp" #include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -278,6 +278,6 @@ int FindOccupiedLaneletID( int FindEgoOccupiedLaneletID(const std::vector & lanelets); } // namespace lib_mission_planner -} +} // namespace autoware::mapless_architecture -#endif // LIB_MISSION_PLANNER__HELPER_FUNCTIONS_HPP_ +#endif // AUTOWARE__LOCAL_MISSION_PLANNER_COMMON__HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml index 6b95f38355bdb..780a0adf34fd4 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml @@ -9,13 +9,13 @@ autoware_cmake + autoware_planning_msgs db_msgs geometry_msgs lanelet2_core tf2 tf2_geometry_msgs tf2_ros - autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index 761a62d956b48..18865b67f9fba 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -3,9 +3,9 @@ #include "autoware/local_mission_planner_common/helper_functions.hpp" #include "lanelet2_core/geometry/Lanelet.h" -#include "autoware_planning_msgs/msg/road_segments.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_planning_msgs/msg/road_segments.hpp" #include "db_msgs/msg/lanelets_stamped.hpp" namespace autoware::mapless_architecture @@ -444,4 +444,4 @@ int FindEgoOccupiedLaneletID(const std::vector & lanelets) } // namespace lib_mission_planner -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp index 9d92ff945cc0b..047b1d616b123 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp @@ -1,11 +1,11 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#ifndef LOCAL_ROAD_PROVIDER_NODE_HPP_ -#define LOCAL_ROAD_PROVIDER_NODE_HPP_ +#ifndef AUTOWARE__LOCAL_ROAD_PROVIDER__LOCAL_ROAD_PROVIDER_NODE_HPP_ +#define AUTOWARE__LOCAL_ROAD_PROVIDER__LOCAL_ROAD_PROVIDER_NODE_HPP_ -#include "autoware_planning_msgs/msg/road_segments.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_planning_msgs/msg/road_segments.hpp" #include "db_msgs/msg/lanelets_stamped.hpp" namespace autoware::mapless_architecture @@ -46,6 +46,6 @@ class LocalRoadProviderNode : public rclcpp::Node rclcpp::Subscription::SharedPtr lanelets_subscriber_; }; -} +} // namespace autoware::mapless_architecture -#endif // LOCAL_ROAD_PROVIDER_NODE_HPP_ +#endif // AUTOWARE__LOCAL_ROAD_PROVIDER__LOCAL_ROAD_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_road_provider/package.xml b/planning/mapless_architecture/autoware_local_road_provider/package.xml index bf975d375e90c..190cd6deb71da 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_road_provider/package.xml @@ -11,10 +11,10 @@ ros2launch - db_msgs autoware_local_mission_planner_common - rclcpp autoware_planning_msgs + db_msgs + rclcpp ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp index 7d25e0d2daf1a..f4fb5b469cfe4 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp @@ -36,4 +36,4 @@ void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::Lanele // Publish the RoadSegments message road_publisher_->publish(road_segments); } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md b/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md index df9b842f28ea1..c2a4ec1e6fb7d 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md +++ b/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md @@ -3,4 +3,5 @@ Converts the selected mission lane to an autoware trajectory. ## Build -In order to build the code, clone the TIER IV version of the autoware ROS messages from https://github.com/tier4/autoware_auto_msgs/tree/tier4/main and include in your build. + +In order to build the code, clone the TIER IV version of the autoware ROS messages from and include in your build. diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp index 200ad281bb5a5..7f080d6147850 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -1,13 +1,13 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#ifndef MISSION_LANE_CONVERTER_NODE_HPP_ -#define MISSION_LANE_CONVERTER_NODE_HPP_ +#ifndef AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_ +#define AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_ -#include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -178,6 +178,6 @@ class MissionLaneConverterNode : public rclcpp::Node // ros parameters float target_speed_; }; -} +} // namespace autoware::mapless_architecture -#endif // MISSION_LANE_CONVERTER_NODE_HPP_ +#endif // AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml index cb7befb96da83..08167412ca035 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml +++ b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml @@ -12,12 +12,12 @@ ros2launch autoware_auto_planning_msgs - geometry_msgs autoware_local_mission_planner_common + autoware_planning_msgs + geometry_msgs rclcpp tf2_geometry_msgs visualization_msgs - autoware_planning_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml b/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml index fd3a755e351d0..e1255342f18dc 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml +++ b/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml @@ -1,4 +1,4 @@ /mission_planner: mission_lane_converter: ros__parameters: - target_speed: 3.2 # [mps] constant target speed of the mission trajectories + target_speed: 3.2 # [mps] constant target speed of the mission trajectories diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 28dc1be16d2ca..9aeea605c09c8 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -611,4 +611,4 @@ visualization_msgs::msg::Marker MissionLaneConverterNode::GetGlobalTrjVisualizat return trj_vis_global; } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp index b18823e3cf351..4c906b0b06a44 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp @@ -20,4 +20,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); } -} +} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp index 75a7b56419958..33af0b6f754ec 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -1,8 +1,8 @@ // Copyright 2024 driveblocks GmbH // driveblocks proprietary license -#include "gtest/gtest.h" #include "autoware/mission_lane_converter/mission_lane_converter_node.hpp" +#include "gtest/gtest.h" #include "geometry_msgs/msg/point.hpp" @@ -112,4 +112,4 @@ int TestMissionToTrajectory() return 0; } -} +} // namespace autoware::mapless_architecture From 71f74696baa7a1e2347ca691689b36b0045f5797 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 20 Jun 2024 13:10:11 +0000 Subject: [PATCH 09/24] Update launch files Signed-off-by: Simon Eisenmann --- .../autoware_hmi/launch/hmi.launch.py | 8 ++++---- .../launch/local_map_provider.launch.py | 10 +++++----- .../launch/mission_planner.launch.py | 12 +++++++----- .../launch/mission_planner_compose.launch.py | 12 ++++++++---- .../launch/local_road_provider.launch.py | 10 +++++----- .../launch/mission_lane_converter.launch.py | 10 +++++----- 6 files changed, 34 insertions(+), 28 deletions(-) diff --git a/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py index c386c34b954e2..86ddd999c250c 100644 --- a/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py +++ b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py @@ -9,10 +9,10 @@ def generate_launch_description(): [ # hmi executable Node( - package="hmi", - executable="hmi", - name="hmi", - namespace="mission_planner", + package="autoware_hmi", + executable="autoware_hmi", + name="autoware_hmi", + namespace="mapless_architecture", remappings=[ ("hmi_node/output/mission", "hmi_node/output/mission"), ], diff --git a/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py index b26c7fd9b7c81..95de7e19abb75 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py +++ b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py @@ -7,12 +7,12 @@ def generate_launch_description(): return LaunchDescription( [ - # local_map_provider executable + # autoware_local_map_provider executable Node( - package="local_map_provider", - executable="local_map_provider", - name="local_map_provider", - namespace="mission_planner", + package="autoware_local_map_provider", + executable="autoware_local_map_provider", + name="autoware_local_map_provider", + namespace="mapless_architecture", remappings=[ ( "local_map_provider_node/output/local_map", diff --git a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py index 1d59f5d3ec7a5..100da396941e0 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py @@ -11,7 +11,9 @@ def generate_launch_description(): mission_planner_param_file = os.path.join( - get_package_share_directory("mission_planner"), "param", "mission_planner_default.yaml" + get_package_share_directory("autoware_local_mission_planner"), + "param", + "mission_planner_default.yaml", ) mission_planner_param = DeclareLaunchArgument( @@ -26,10 +28,10 @@ def generate_launch_description(): mission_planner_param, # mission_planner executable Node( - package="mission_planner", - executable="mission_planner", - name="mission_planner", - namespace="mission_planner", + package="autoware_local_mission_planner", + executable="autoware_local_mission_planner", + name="autoware_local_mission_planner", + namespace="mapless_architecture", remappings=[ ( "mission_planner_node/output/marker", diff --git a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py index 85028833bef37..0cab6abcafd47 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py @@ -21,7 +21,7 @@ def generate_launch_description(): # https://github.com/ros2/launch/issues/618 # https://gitlab.com/driveblocks/mod_feature_detection/-/merge_requests/102/diffs mission_lane_converter_pkg_share_directory = get_package_share_directory( - "mission_lane_converter" + "autoware_mission_lane_converter" ) mission_lane_converter_file_name = "mission_lane_converter.launch.py" mission_lane_converter_path = ( @@ -35,7 +35,7 @@ def generate_launch_description(): # launch_arguments={'node_name': 'bar'}.items() ) - hmi_pkg_share_directory = get_package_share_directory("hmi") + hmi_pkg_share_directory = get_package_share_directory("autoware_hmi") hmi_file_name = "hmi.launch.py" hmi_path = Path(hmi_pkg_share_directory) / "launch/" / hmi_file_name @@ -44,7 +44,9 @@ def generate_launch_description(): # launch_arguments={'node_name': 'bar'}.items() ) - local_road_provider_pkg_share_directory = get_package_share_directory("local_road_provider") + local_road_provider_pkg_share_directory = get_package_share_directory( + "autoware_local_road_provider" + ) local_road_provider_file_name = "local_road_provider.launch.py" local_road_provider_path = ( Path(local_road_provider_pkg_share_directory) / "launch/" / local_road_provider_file_name @@ -55,7 +57,9 @@ def generate_launch_description(): # launch_arguments={'node_name': 'bar'}.items() ) - local_map_provider_pkg_share_directory = get_package_share_directory("local_map_provider") + local_map_provider_pkg_share_directory = get_package_share_directory( + "autoware_local_map_provider" + ) local_map_provider_file_name = "local_map_provider.launch.py" local_map_provider_path = ( Path(local_map_provider_pkg_share_directory) / "launch/" / local_map_provider_file_name diff --git a/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py b/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py index 530c57745a110..ffb0293816857 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py +++ b/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py @@ -7,12 +7,12 @@ def generate_launch_description(): return LaunchDescription( [ - # local_road_provider executable + # autoware_local_road_provider executable Node( - package="local_road_provider", - executable="local_road_provider", - name="local_road_provider", - namespace="mission_planner", + package="autoware_local_road_provider", + executable="autoware_local_road_provider", + name="autoware_local_road_provider", + namespace="mapless_architecture", remappings=[ ( "local_road_provider_node/output/road_segments", diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py index 74c39a1884f71..6b35a12545828 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py +++ b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py @@ -11,7 +11,7 @@ def generate_launch_description(): mission_lane_converter_param_file = os.path.join( - get_package_share_directory("mission_lane_converter"), + get_package_share_directory("autoware_mission_lane_converter"), "param", "mission_lane_converter_default.yaml", ) @@ -28,10 +28,10 @@ def generate_launch_description(): mission_lane_converter_param, # mission lane converter executable Node( - package="mission_lane_converter", - executable="mission_lane_converter", - name="mission_lane_converter", - namespace="mission_planner", + package="autoware_mission_lane_converter", + executable="autoware_mission_lane_converter", + name="autoware_mission_lane_converter", + namespace="mapless_architecture", remappings=[ ( "mission_lane_converter/input/mission_lanes", From 5033efb73f130fc7378f78e241cf4b686316401f Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Mon, 24 Jun 2024 16:41:23 +0200 Subject: [PATCH 10/24] Change readme Signed-off-by: Simon Eisenmann --- planning/mapless_architecture/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/mapless_architecture/README.md b/planning/mapless_architecture/README.md index d7ad77a39f15b..68d08b7067eb8 100644 --- a/planning/mapless_architecture/README.md +++ b/planning/mapless_architecture/README.md @@ -24,13 +24,13 @@ The Mission Planner consists of several components (ROS2 packages) working toget To launch all nodes of the software: ``` -ros2 launch mission_planner mission_planner_compose.launch.py +ros2 launch autoware_local_mission_planner mission_planner_compose.launch.py ``` To launch a specific node, such as the mission planner: ``` -ros2 launch mission_planner mission_planner.launch.py +ros2 launch autoware_local_mission_planner mission_planner.launch.py ``` ## Additional Notes From b43176011c69b48651634de606855c1ef5b22907 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Mon, 24 Jun 2024 14:48:40 +0000 Subject: [PATCH 11/24] Specify language Signed-off-by: Simon Eisenmann --- planning/mapless_architecture/README.md | 6 +++--- planning/mapless_architecture/autoware_hmi/Readme.md | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/mapless_architecture/README.md b/planning/mapless_architecture/README.md index 68d08b7067eb8..caf496065ccdf 100644 --- a/planning/mapless_architecture/README.md +++ b/planning/mapless_architecture/README.md @@ -23,13 +23,13 @@ The Mission Planner consists of several components (ROS2 packages) working toget To launch all nodes of the software: -``` +```bash ros2 launch autoware_local_mission_planner mission_planner_compose.launch.py ``` To launch a specific node, such as the mission planner: -``` +```bash ros2 launch autoware_local_mission_planner mission_planner.launch.py ``` @@ -41,7 +41,7 @@ During the beta phase, the mission planner will immediately output a straight tr This package adheres to the [Autoware styleguide](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/languages/cpp/) which can be achieved via [`pre-commit`](https://autowarefoundation.github.io/autoware-documentation/pr-347/contributing/pull-request-guidelines/ci-checks/#pre-commit). Run -``` +```bash pre-commit run -a ``` diff --git a/planning/mapless_architecture/autoware_hmi/Readme.md b/planning/mapless_architecture/autoware_hmi/Readme.md index c7d6875070def..d84dd5231b921 100644 --- a/planning/mapless_architecture/autoware_hmi/Readme.md +++ b/planning/mapless_architecture/autoware_hmi/Readme.md @@ -12,6 +12,6 @@ Available missions: Interact with this node by changing the ROS parameters. For a lane change to the right use this command in the terminal: -``` +```bash ros2 param set /mission_planner/hmi mission LANE_CHANGE_RIGHT ``` From 4af9b99e76ad1d6ee3a99bdc8b387217834cf758 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Tue, 25 Jun 2024 09:43:09 +0000 Subject: [PATCH 12/24] Fix pre-commit issues Signed-off-by: Simon Eisenmann --- .../autoware/local_mission_planner/mission_planner_node.hpp | 1 - .../src/mission_planner_node.cpp | 1 - .../local_mission_planner_common/helper_functions.hpp | 4 +--- .../src/helper_functions.cpp | 4 ---- .../src/mission_lane_converter_node.cpp | 4 ++-- 5 files changed, 3 insertions(+), 11 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 54e3b3a1160da..92d834b9959f5 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -29,7 +29,6 @@ namespace autoware::mapless_architecture { -using namespace lib_mission_planner; // Create Direction data type typedef int Direction; diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 6040359cd5999..fed74e3e0a9fd 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -30,7 +30,6 @@ namespace autoware::mapless_architecture { using std::placeholders::_1; -using namespace lib_mission_planner; MissionPlannerNode::MissionPlannerNode() : Node("mission_planner_node") { diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index 4151dae5d3e49..10c2b5801c88a 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -14,12 +14,11 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include namespace autoware::mapless_architecture { -namespace lib_mission_planner -{ /** * @brief A class for a 2D pose. @@ -277,7 +276,6 @@ int FindOccupiedLaneletID( */ int FindEgoOccupiedLaneletID(const std::vector & lanelets); -} // namespace lib_mission_planner } // namespace autoware::mapless_architecture #endif // AUTOWARE__LOCAL_MISSION_PLANNER_COMMON__HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index 18865b67f9fba..68bd3878cc8f6 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -10,8 +10,6 @@ namespace autoware::mapless_architecture { -namespace lib_mission_planner -{ double NormalizePsi(const double psi) { @@ -442,6 +440,4 @@ int FindEgoOccupiedLaneletID(const std::vector & lanelets) return FindOccupiedLaneletID(lanelets, position_ego); } -} // namespace lib_mission_planner - } // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 9aeea605c09c8..c4662c2842a81 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -19,7 +19,6 @@ namespace autoware::mapless_architecture { using std::placeholders::_1; -using namespace lib_mission_planner; MissionLaneConverterNode::MissionLaneConverterNode() : Node("mission_lane_converter_node") { @@ -453,7 +452,8 @@ void MissionLaneConverterNode::AddHeadingToTrajectory_( return; } -// TODO: store the latest odometry message here and then re-use in the output conversion +// TODO(thomas.herrmann@driveblocks.ai): store the latest odometry message here and then re-use in +// the output conversion void MissionLaneConverterNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg) { // store current odometry information From 4a8b5863692c64dfa1d741edffb9f02f6244050e Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Tue, 25 Jun 2024 11:32:14 +0000 Subject: [PATCH 13/24] Work on documentation Signed-off-by: Simon Eisenmann --- .../include/autoware/hmi/hmi_node.hpp | 7 - .../autoware_hmi/src/hmi_node.cpp | 8 - .../local_map_provider_node.hpp | 7 - .../mission_planner_node.hpp | 19 +-- .../test/src/test_mission_planner_core.cpp | 17 +- .../helper_functions.hpp | 43 +++-- .../src/helper_functions.cpp | 100 ++++++----- .../local_road_provider_node.hpp | 7 - .../mission_lane_converter_node.hpp | 51 +++--- .../src/mission_lane_converter_node.cpp | 157 +++++++++--------- .../test_mission_planner_converter.hpp | 7 +- .../src/test_mission_planner_converter.cpp | 26 +-- 12 files changed, 217 insertions(+), 232 deletions(-) diff --git a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp index 82e97d80cf42a..efb9726683686 100644 --- a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp +++ b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp @@ -27,10 +27,6 @@ class HMINode : public rclcpp::Node HMINode(); private: - // ########################################################################### - // # PRIVATE PROCESSING METHODS - // ########################################################################### - /** * @brief Callback function for parameter changes. * This callback function is triggered whenever a ROS 2 parameter is changed. @@ -46,9 +42,6 @@ class HMINode : public rclcpp::Node */ void PublishMission_(std::string mission); - // ########################################################################### - // # PRIVATE VARIABLES - // ########################################################################### // Declare ROS2 publisher and subscriber rclcpp::Publisher::SharedPtr mission_publisher_; diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index 33f68fc58734d..bf0add6ece583 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -11,14 +11,6 @@ namespace autoware::mapless_architecture { using std::placeholders::_1; -/** - -* @brief Constructor for the HMINode class. - -* Initializes the publisher and subscriber with appropriate topics and QoS -settings. - -*/ HMINode::HMINode() : Node("hmi_node") { // Set quality of service to best effort (if transmission fails, do not try to diff --git a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp index c684da39e79ac..5c34b61c161fd 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp @@ -26,10 +26,6 @@ class LocalMapProviderNode : public rclcpp::Node LocalMapProviderNode(); private: - // ########################################################################### - // # PRIVATE PROCESSING METHODS - // ########################################################################### - /** * @brief The callback for the RoadSegments messages. * @@ -37,9 +33,6 @@ class LocalMapProviderNode : public rclcpp::Node */ void CallbackRoadSegmentsMessages_(const autoware_planning_msgs::msg::RoadSegments & msg); - // ########################################################################### - // # PRIVATE VARIABLES - // ########################################################################### // Declare ROS2 publisher and subscriber rclcpp::Publisher::SharedPtr map_publisher_; diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 92d834b9959f5..7688c6016ba14 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -30,7 +30,7 @@ namespace autoware::mapless_architecture { -// Create Direction data type +// Direction data type typedef int Direction; const Direction stay = 0; const Direction left = 1; @@ -38,6 +38,7 @@ const Direction right = 2; const Direction left_most = 3; const Direction right_most = 4; +// Define Lanes struct Lanes { std::vector ego; @@ -204,10 +205,6 @@ class MissionPlannerNode : public rclcpp::Node std::vector & out_lanelet_connections); private: - // ########################################################################### - // # PRIVATE PROCESSING METHODS - // ########################################################################### - /** * @brief Function for the visualization of lanes. * @@ -283,9 +280,6 @@ class MissionPlannerNode : public rclcpp::Node */ void CalculatePredecessors(std::vector & lanelet_connections); - // ########################################################################### - // # PRIVATE VARIABLES - // ########################################################################### // Declare ROS2 publisher and subscriber rclcpp::Subscription::SharedPtr mapSubscriber_; @@ -309,11 +303,12 @@ class MissionPlannerNode : public rclcpp::Node // ROS buffer interface (for TF transforms) std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - // store previous odometry message + + // Store previous odometry message nav_msgs::msg::Odometry last_odom_msg_; - Pose2D pose_prev_; - // Pose2D target_pose_ = {0.0, 0.0}; + // Initialize some variables + Pose2D pose_prev_; bool pose_prev_init_ = false; bool b_global_odometry_deprecation_warning_ = false; bool received_motion_update_once_ = false; @@ -329,7 +324,7 @@ class MissionPlannerNode : public rclcpp::Node std::vector lane_right_; std::vector current_lanelets_; - // ros parameters + // ROS parameters float distance_to_centerline_threshold_; float projection_distance_on_goallane_; int retrigger_attempts_max_; diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index c83408737130e..eacb9914a6edf 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -127,6 +127,7 @@ int TestCalculateDistanceBetweenPointAndLineString() { // Create an example Linestring lanelet::LineString2d linestring; + lanelet::Point2d p1(0, 1.0, 0.0); // First argument is ID (set it to 0 for now) linestring.push_back(p1); @@ -241,7 +242,7 @@ db_msgs::msg::LaneletsStamped GetTestRoadModelForRecenterTests() std::vector lanelet_vec(n_lanelets); message.lanelets = lanelet_vec; - // global position + // Global position geometry_msgs::msg::PoseStamped msg_global_pose; msg_global_pose.header.frame_id = "base_link"; @@ -267,7 +268,7 @@ db_msgs::msg::LaneletsStamped GetTestRoadModelForRecenterTests() tf2::Quaternion quaternion; - // vehicle position in global cosy + // Vehicle position in global cosy message.pose.position.x = 0.0; message.pose.position.y = 0.0; quaternion.setRPY(0, 0, 0); @@ -276,7 +277,7 @@ db_msgs::msg::LaneletsStamped GetTestRoadModelForRecenterTests() message.pose.orientation.z = quaternion.getZ(); message.pose.orientation.w = quaternion.getW(); - // street layout in current local cosy = global cosy since vehicle position is + // Street layout in current local cosy = global cosy since vehicle position is // at the origin in the global cosy message.lanelets[0].linestrings[0].points[0].pose.position.x = 0.0; message.lanelets[0].linestrings[0].points[0].pose.position.y = -0.5; @@ -349,7 +350,7 @@ int TestRecenterGoalpoint() goal_point.y() = 0.25; goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); - // expect re-centered point to lie on y coordinate 0 + // Expect re-centered point to lie on y coordinate 0 EXPECT_NEAR(goal_point_recentered.x(), goal_point.x(), 1e-5); EXPECT_NEAR(goal_point_recentered.y(), 0.0, 1e-5); @@ -359,7 +360,7 @@ int TestRecenterGoalpoint() goal_point.y() = -0.2; goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); - // expect re-centered point to lie on y coordinate 0 but with x coord equal to + // Expect re-centered point to lie on y coordinate 0 but with x coord equal to // goal_point (removes lateral error/noise) EXPECT_NEAR(goal_point_recentered.x(), goal_point.x(), 1e-5); EXPECT_NEAR(goal_point_recentered.y(), 0.0, 1e-5); @@ -371,7 +372,7 @@ int TestRecenterGoalpoint() goal_point.y() = 1.75; goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); - // expect re-centered point to lie on y coordinate 0 but with x coord equal to + // Expect re-centered point to lie on y coordinate 0 but with x coord equal to // goal_point (removes lateral error/noise) EXPECT_NEAR(goal_point_recentered.x(), goal_point.x(), 1e-5); EXPECT_NEAR(goal_point_recentered.y(), 1.5, 1e-5); @@ -416,7 +417,7 @@ int TestCheckIfGoalPointShouldBeReset() lanelet::BasicPoint2d point(-1.0, 0.0); MissionPlanner.goal_point(point); - // set a non-default mission to make the goal point reset work + // Set a non-default mission to make the goal point reset work autoware_planning_msgs::msg::Mission mission_msg; mission_msg.mission_type = autoware_planning_msgs::msg::Mission::LANE_CHANGE_LEFT; MissionPlanner.CallbackMissionMessages_(mission_msg); @@ -433,7 +434,7 @@ int TestCheckIfGoalPointShouldBeReset() // TEST 2: check if goal point reset is skipped in default mission MissionPlanner.goal_point(point); - // set a non-default mission to make the goal point reset work + // Set a non-default mission to make the goal point reset work mission_msg.mission_type = autoware_planning_msgs::msg::Mission::LANE_KEEP; MissionPlanner.CallbackMissionMessages_(mission_msg); diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index 10c2b5801c88a..ce86ea0377da7 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -30,7 +30,7 @@ class Pose2D Pose2D(); Pose2D(const double x, const double y, const double psi = 0.0); - // accessors and mutators + // Accessors and mutators double get_x() const; double get_y() const; Eigen::Vector2d get_xy() const; @@ -43,7 +43,7 @@ class Pose2D void set_psi(const double psi); private: - // data variables + // Data variables Eigen::Vector2d xy_; double psi_; }; @@ -57,9 +57,9 @@ class Pose2D * providing the absolute pose of the new cosy as "pose_prev" and the * absolute pose of the old/current cosy as "cosy_rel". * - * @param cosy_rel translation and rotation between the current/old cosy and + * @param cosy_rel Translation and rotation between the current/old cosy and * a new/go-to cosy - * @param pose_prev coordinates and heading of a pose in the current/old cosy + * @param pose_prev Coordinates and heading of a pose in the current/old cosy * @return Pose coordinates and heading of pose_prev in the new cosy * (defined by the shift cosy_rel between previous and current cosy) */ @@ -148,7 +148,7 @@ std::vector> GetAllSuccessorSequences( * @param id_initial_lanelet ID of lanelet from where neighbor search is started * @param adjacent_lane_type Specifies whether predecessors or successors -should be targeted (e.g. lanelet_tools::AdjacentLaneType::kPredecessors) +should be targeted (e.g. lanelet_tools::AdjacentLaneType::kPredecessors) * * @return Collection of sequences of all adjacent lanelets */ @@ -157,18 +157,16 @@ std::vector> GetAllLaneletSequences( const AdjacentLaneType adjacent_lane_type); /** -* @brief Find relevant adjacent (successors or predecessors) lanelets - (currently relevant means leading towards goal) among a set of -provided adjacent lanelets (ids_adjacent_lanelets) +* @brief Find relevant adjacent (successors or predecessors) lanelets (currently relevant means +leading towards goal) among a set of provided adjacent lanelets (ids_adjacent_lanelets) * * @param lanelet_connections Relation between individual lanelets (successors/neighbors) * @param ids_adjacent_lanelets IDs of all available adjacent lanelets (either successors or predecessors) * @param do_include_navigation_info Whether to use navigation info to -* determine relevant successors (true) or not (false); if -navigation info is not used, the ID of the first direct successors will be - returned +* determine relevant successors (true) or not (false); if navigation info is not used, +the ID of the first direct successors will be returned * @return ID of relevant successor lanelet */ std::vector GetRelevantAdjacentLanelets( @@ -179,11 +177,10 @@ std::vector GetRelevantAdjacentLanelets( * @brief Get a complete lanelet ID sequence starting from an initial lanelet. * * @param lanelet_id_sequence_current Current lanelet ID sequence (of - * previous iteration); this is the start for the search in the current - * iteration + * previous iteration); this is the start for the search in the current iteration * @param lanelets_already_visited List of already visited lanelet IDs - * @param ids_relevant_lanelets IDs of the relevant adjacent - * (successor or predecessor) lanelets + * @param ids_relevant_lanelets IDs of the relevant adjacent (successor or predecessor) + * lanelets * @param id_initial_lanelet ID of lanelet from which search was * started initially * @return - Vector containing IDs of a completed lanelet sequence; is empty @@ -197,7 +194,7 @@ std::tuple, bool> GetCompletedLaneletSequence( const std::vector ids_relevant_lanelets, const int id_initial_lanelet); /** - * @brief The vehicle side. + * @brief The vehicle side (left or right). * */ enum VehicleSide { kLeft = 0, kRight = 1 }; @@ -231,15 +228,13 @@ std::vector GetAllNeighboringLaneletIDs( * determine relevant neighbors (true) or * not (false) * @param recursiveness (defaults to 1) - * level of recursiveness for neighbor search - * - recursivenes=-1 means that the most outside lanes are requested - (returns initial lanelet ID if no neighbors are available -> initial lanelet - is the most outside lane) - * - recursiveness>=0 means that the direct neighbors (1), neighbors of - neighbors (2), ..., are searched (returns -1 if no lanelet is - available at the specified level of recursiveness) + * - level of recursiveness for neighbor search + * - recursivenes=-1 means that the most outside lanes are requested (returns initial lanelet ID + if no neighbors are available -> initial lanelet is the most outside lane) + * - recursiveness>=0 means that the direct neighbors (1), neighbors of neighbors (2), ..., are + searched (returns -1 if no lanelet is available at the specified level of recursiveness) * - * @return ID of neighboring lanelet (returns -1 if no neighbor available) + * @return ID of neighboring lanelet (returns -1 if no neighbor available) */ int GetNeighboringLaneletID( const std::vector & lanelet_connections, const int id_initial_lanelet, diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index 68bd3878cc8f6..b65bc90c264f1 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -13,10 +13,10 @@ namespace autoware::mapless_architecture double NormalizePsi(const double psi) { - // remove multiples of 2 PI by using modulo on absolute value and apply sign + // Remove multiples of 2 PI by using modulo on absolute value and apply sign double psi_out = copysign(fmod(fabs(psi), M_PI * 2.0), psi); - // restrict psi to [-pi, pi[ + // Restrict psi to [-pi, pi[ if (psi_out >= M_PI) psi_out -= 2.0 * M_PI; else if (psi_out < -M_PI) @@ -30,22 +30,22 @@ std::vector GetPsiForPoints(const std::vector int num_points = points.size(); std::vector tang_vecs(num_points * 2); - // get heading vector for first point + // Get heading vector for first point tang_vecs[0] = points[1].x - points[0].x; tang_vecs[1] = points[1].y - points[0].y; - // use one point before and after the targeted one for heading vector (more + // Use one point before and after the targeted one for heading vector (more // stable/robust than relying on a single pair of points) for (int i = 1; i < num_points - 1; i++) { tang_vecs[2 * i] = points[i + 1].x - points[i - 1].x; tang_vecs[2 * i + 1] = points[i + 1].y - points[i - 1].y; } - // get heading vector for last point + // Get heading vector for last point tang_vecs[2 * (num_points - 1)] = points[num_points - 1].x - points[num_points - 2].x; tang_vecs[2 * (num_points - 1) + 1] = points[num_points - 1].y - points[num_points - 2].y; - // calculate heading angles with atan2 + // Calculate heading angles with atan2 std::vector psi_points(num_points); for (int i = 0; i < num_points; i++) { psi_points[i] = NormalizePsi(std::atan2(tang_vecs[2 * i + 1], tang_vecs[2 * i])); @@ -59,27 +59,33 @@ Pose2D::Pose2D() this->set_xy(0.0, 0.0); this->set_psi(0.0); } + Pose2D::Pose2D(const double x, const double y, const double psi /* = 0.0 */) { this->set_xy(x, y); this->set_psi(psi); } + double Pose2D::get_x() const { return this->xy_(0); } + double Pose2D::get_y() const { return this->xy_(1); } + Eigen::Vector2d Pose2D::get_xy() const { return this->xy_; } + double Pose2D::get_psi() const { return this->psi_; } + geometry_msgs::msg::Point Pose2D::get_point() const { geometry_msgs::msg::Point tmp_point; @@ -87,22 +93,27 @@ geometry_msgs::msg::Point Pose2D::get_point() const tmp_point.y = this->xy_(1); return tmp_point; } + void Pose2D::set_x(const double x) { this->xy_(0) = x; } + void Pose2D::set_y(const double y) { this->xy_(1) = y; } + void Pose2D::set_xy(const double x, const double y) { this->xy_ = {x, y}; } + void Pose2D::set_xy(const Eigen::Vector2d xy) { this->xy_ = xy; } + void Pose2D::set_psi(const double psi) { this->psi_ = psi; @@ -189,7 +200,7 @@ std::vector> GetAllLaneletSequences( const std::vector & lanelet_connections, const int id_initial_lanelet, const AdjacentLaneType adjacent_lane_type) { - // initialize helper variables + // Initialize helper variables bool do_include_navigation_info = false; std::vector lanelets_already_visited; @@ -197,7 +208,7 @@ std::vector> GetAllLaneletSequences( std::vector> lanelet_sequences; - // loop as long as all successors of the initial lanelet have been searched + // Loop as long as all successors of the initial lanelet have been searched // maximum iteration depth is (number_of_lanelets - 1) * 2 for (size_t i = 0; i < lanelet_connections.size() * 2; i++) { // IDs which are relevant for searching adjacent lanelets (either successors @@ -221,7 +232,7 @@ std::vector> GetAllLaneletSequences( if (do_exit_outer_for_loop) break; - // store returned complete lanelet id sequence + // Store returned complete lanelet id sequence if (!lanelet_id_sequence_completed.empty()) { lanelet_sequences.push_back(lanelet_id_sequence_completed); } @@ -235,19 +246,19 @@ std::vector GetRelevantAdjacentLanelets( { std::vector ids_relevant_successors; - // return all successors if navigation info is not relevant + // Return all successors if navigation info is not relevant if (do_include_navigation_info) { - // loop through all successors and check if they lead towards the navigation + // Loop through all successors and check if they lead towards the navigation // goal if (ids_adjacent_lanelets.front() >= 0) { for (std::size_t i = 0; i < ids_adjacent_lanelets.size(); i++) { - // check if successor leads to goal + // Check if successor leads to goal if (lanelet_connections[ids_adjacent_lanelets[i]].goal_information) { ids_relevant_successors.push_back(ids_adjacent_lanelets[i]); } } } - // default return: valid successor lanelet not available + // Default return: valid successor lanelet not available if (ids_relevant_successors.empty()) { ids_relevant_successors.push_back(-1); } @@ -256,9 +267,7 @@ std::vector GetRelevantAdjacentLanelets( } // FIXME: avoid that list is empty -> has to be -1 if no relevant lanelet - // exists - // this fixes an issue that originates in the lanelet converter; should be - // fixed there! + // exists, this fixes an issue that originates in the lanelet converter; should be fixed there! if (ids_relevant_successors.empty()) { ids_relevant_successors.push_back(-1); } @@ -273,47 +282,47 @@ std::tuple, bool> GetCompletedLaneletSequence( std::vector lanelet_id_sequence_completed; bool do_exit_outer_for_loop = false; - // check if an adjacent lanelet is even available + // Check if an adjacent lanelet is even available if (ids_relevant_lanelets.front() >= 0) { bool has_relevant_successor = false; - // loop though all relevant adjacent IDs and check whether or not they + // Loop though all relevant adjacent IDs and check whether or not they // have already been visited for (const int & successor_id : ids_relevant_lanelets) { if ( std::find(lanelets_already_visited.begin(), lanelets_already_visited.end(), successor_id) == lanelets_already_visited.end()) { - // add new ID to already visited vector + // Add new ID to already visited vector lanelets_already_visited.push_back(successor_id); - // add currently visited ID to temporary adjacent lanelet sequence + // Add currently visited ID to temporary adjacent lanelet sequence lanelet_id_sequence_current.push_back(successor_id); has_relevant_successor = true; break; } } - // if all available adjacent lanelets were already visited, go back to + // If all available adjacent lanelets were already visited, go back to // parent lanelet or exit loop if (!has_relevant_successor) { - // pop parent lanelet except it is already the initial lanelet + // Pop parent lanelet except it is already the initial lanelet if (lanelet_id_sequence_current.back() != id_initial_lanelet) { lanelet_id_sequence_current.pop_back(); } else { - // exit loop if initial lanelet has no relevant adjacent lanelets left + // Exit loop if initial lanelet has no relevant adjacent lanelets left do_exit_outer_for_loop = true; } } } else { - // exit loop if initial lanelet has already no relevant adjacent lanelets + // Exit loop if initial lanelet has already no relevant adjacent lanelets if (lanelet_id_sequence_current.back() == id_initial_lanelet) { do_exit_outer_for_loop = true; } else { - // store lanelet sequence when it is completed (no unvisited adjacent + // Store lanelet sequence when it is completed (no unvisited adjacent // lanelets left from current lanelet) lanelet_id_sequence_completed = lanelet_id_sequence_current; - // go back to parent lanelet ID and continue search from there + // Go back to parent lanelet ID and continue search from there lanelet_id_sequence_current.pop_back(); } } @@ -327,24 +336,23 @@ std::vector GetAllNeighboringLaneletIDs( int id_current_lanelet = id_initial_lanelet; std::vector lanelet_id_neighbors; - // this function is only intended to return all neighboring lanelets, not only + // This function is only intended to return all neighboring lanelets, not only // the ones leading towards goal. Therefore, this flag is set false for the // sub-function. const bool do_include_navigation_info = false; - // loop as long as all left or right neighbors of the initial lanelet have - // been searched - // maximum iteration depth is: number_of_lanelets + // Loop as long as all left or right neighbors of the initial lanelet have + // been searched, maximum iteration depth is: number_of_lanelets for (size_t i = 0; i < lanelet_connections.size(); i++) { int idx_tmp = GetNeighboringLaneletID( lanelet_connections, id_current_lanelet, side, do_include_navigation_info); - // if ID >= 0, continue search, else break + // If ID >= 0, continue search, else break if (idx_tmp >= 0) { id_current_lanelet = idx_tmp; lanelet_id_neighbors.push_back(id_current_lanelet); } else { - // if return vector is still empty because no neighbors are available, + // If return vector is still empty because no neighbors are available, // return -1 if (lanelet_id_neighbors.empty()) { lanelet_id_neighbors.push_back(-1); @@ -360,30 +368,30 @@ int GetNeighboringLaneletID( const std::vector & lanelet_connections, const int id_initial_lanelet, const VehicleSide side, const bool do_include_navigation_info, const int recursiveness) { - // set default return id if initial lane has already no neighbors + // Set default return id if initial lane has already no neighbors int id_neighbor_lanelet = -1; - // check if most outside lane is searched - // here: return current lanelet's ID when no neighbors are available + // Check if most outside lane is searched + // Here: return current lanelet's ID when no neighbors are available if (recursiveness == -1) { id_neighbor_lanelet = id_initial_lanelet; } - // get ID of current lanelet's neighboring lanelet + // Get ID of current lanelet's neighboring lanelet int id_neighbor_lanelet_tmp = lanelet_connections[id_initial_lanelet].neighbor_lanelet_ids[side]; - // init counter to track current level of recursiveness in loop + // Init counter to track current level of recursiveness in loop int recursiveness_counter = 0; - // loop as long as all neighbors of the initial lanelet have been searched - // maximum iteration depth: number_of_lanelets + // Loop as long as all neighbors of the initial lanelet have been searched, maximum iteration + // depth: number_of_lanelets for (size_t i = 0; i < lanelet_connections.size(); i++) { - // break while loop if desired recursiveness is reached + // Break while loop if desired recursiveness is reached if (recursiveness != -1 && recursiveness_counter >= recursiveness) break; - // check if neighbor is available + // Check if neighbor is available if (id_neighbor_lanelet_tmp >= 0) { - // break if navigation info is considered AND the lanelet does not lead + // Break if navigation info is considered AND the lanelet does not lead // towards goal if ( do_include_navigation_info && @@ -391,14 +399,14 @@ int GetNeighboringLaneletID( break; } - // store current neighbor lanelet ID + // Store current neighbor lanelet ID id_neighbor_lanelet = id_neighbor_lanelet_tmp; - // query neighbor of neighbor and use as new start lanelet + // Query neighbor of neighbor and use as new start lanelet id_neighbor_lanelet_tmp = lanelet_connections[id_neighbor_lanelet_tmp].neighbor_lanelet_ids[side]; } else { - // return -1 if lanelet ID at specific recursiveness is requested + // Return -1 if lanelet ID at specific recursiveness is requested if (recursiveness >= 0) { id_neighbor_lanelet = -1; } @@ -422,7 +430,7 @@ int FindOccupiedLaneletID( { int id_occupied_lanelet = -1; - // check if position is within one of the available lanelet + // Check if position is within one of the available lanelet for (size_t i = 0; i < lanelets.size(); i++) { if (lanelet::geometry::inside(lanelets[i], position)) { id_occupied_lanelet = i; diff --git a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp index 047b1d616b123..52cd721c62923 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp @@ -26,10 +26,6 @@ class LocalRoadProviderNode : public rclcpp::Node LocalRoadProviderNode(); private: - // ########################################################################### - // # PRIVATE PROCESSING METHODS - // ########################################################################### - /** * @brief The callback for the LaneletsStamped messages. * @@ -37,9 +33,6 @@ class LocalRoadProviderNode : public rclcpp::Node */ void CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg); - // ########################################################################### - // # PRIVATE VARIABLES - // ########################################################################### // Declare ROS2 publisher and subscriber rclcpp::Publisher::SharedPtr road_publisher_; diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp index 7f080d6147850..2807b6c1ab834 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -27,19 +27,14 @@ class MissionLaneConverterNode : public rclcpp::Node public: MissionLaneConverterNode(); - // ########################################################################### - // # PUBLIC METHODS - // ########################################################################### - /** * @brief Converts the mission message into a reference trajectory which is * forwarded to a local trajectory planner for refinement. * * @param msg The mission lanes * @return std::tuple + * visualization_msgs::msg::Marker, autoware_auto_planning_msgs::msg::Path, + * visualization_msgs::msg::Marker> */ std::tuple< autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, @@ -47,10 +42,6 @@ class MissionLaneConverterNode : public rclcpp::Node ConvertMissionToTrajectory(const autoware_planning_msgs::msg::MissionLanesStamped & msg); private: - // ########################################################################### - // # PRIVATE PROCESSING METHODS - // ########################################################################### - /** * @brief Computes a trajectory based on the mission planner input. * @@ -99,14 +90,39 @@ class MissionLaneConverterNode : public rclcpp::Node */ void TimedStartupTrajectoryCallback(); + /** + *@brief Create a path bound. + * + *@param bound_path The path. + *@param path_vis The visualization marker. + *@param bound_mission_lane The mission lane. + *@param id_marker The ID marker. + */ void CreatePathBound_( std::vector & bound_path, visualization_msgs::msg::Marker & path_vis, const std::vector & bound_mission_lane, const int id_marker); + /** + *@brief Add a path point. + * + *@param pth_msg The path. + *@param x The x value. + *@param y The y value. + *@param v_x The v_x value. + */ void AddPathPoint_( autoware_auto_planning_msgs::msg::Path & pth_msg, const double x, const double y, const double v_x); + /** + *@brief Create a motion planner input. + * + *@param trj_msg The trajectory. + *@param path_msg The path. + *@param trj_vis The visualization marker for the trajectory. + *@param path_vis The visualization marker for the path. + *@param centerline_mission_lane The centerline of the mission lane. + */ void CreateMotionPlannerInput_( autoware_auto_planning_msgs::msg::Trajectory & trj_msg, autoware_auto_planning_msgs::msg::Path & path_msg, visualization_msgs::msg::Marker & trj_vis, @@ -140,9 +156,6 @@ class MissionLaneConverterNode : public rclcpp::Node visualization_msgs::msg::Marker GetGlobalTrjVisualization_( const autoware_auto_planning_msgs::msg::Trajectory & trj_msg); - // ########################################################################### - // # PRIVATE VARIABLES - // ########################################################################### // Declare ROS2 publisher and subscriber rclcpp::Subscription::SharedPtr odom_subscriber_; @@ -165,17 +178,17 @@ class MissionLaneConverterNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - // switch to print a warning about wrongly configured odometry frames + // Switch to print a warning about wrongly configured odometry frames bool b_global_odometry_deprecation_warning_ = false; bool received_motion_update_once_ = false; - // store initial and last available odom messages + + // Store initial and last available odom messages nav_msgs::msg::Odometry last_odom_msg_, initial_odom_msg_; - // workaround to start the vehicle driving into the computed local road - // model + // Workaround to start the vehicle driving into the computed local road model bool mission_lanes_available_once_ = false; - // ros parameters + // ROS parameters float target_speed_; }; } // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index c4662c2842a81..381d28dd9fb7b 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -48,11 +48,11 @@ MissionLaneConverterNode::MissionLaneConverterNode() : Node("mission_lane_conver this->create_publisher( "mission_lane_converter/output/global_trajectory", qos_reliability); - // artificial publisher to test the trajectory generation + // Artificial publisher to test the trajectory generation publisher_ = this->create_publisher( "mission_lane_converter/output/global_trajectory", qos_reliability); - // path publisher + // Path publisher path_publisher_ = create_publisher( "mission_lane_converter/output/path", qos_reliability); @@ -83,11 +83,11 @@ MissionLaneConverterNode::MissionLaneConverterNode() : Node("mission_lane_conver void MissionLaneConverterNode::TimedStartupTrajectoryCallback() { if (!mission_lanes_available_once_) { - // empty trajectory for controller + // Empty trajectory for controller autoware_auto_planning_msgs::msg::Trajectory trj_msg = autoware_auto_planning_msgs::msg::Trajectory(); - // frame id must be "map" for Autoware controller + // Frame id must be "map" for Autoware controller trj_msg.header.frame_id = "map"; trj_msg.header.stamp = rclcpp::Node::now(); @@ -99,7 +99,7 @@ void MissionLaneConverterNode::TimedStartupTrajectoryCallback() AddTrajectoryPoint_(trj_msg, x, y, v_x); } - // heading in trajectory path will be overwritten + // Heading in trajectory path will be overwritten this->AddHeadingToTrajectory_(trj_msg); trj_msg = TransformToGlobalFrame(trj_msg); @@ -114,9 +114,9 @@ void MissionLaneConverterNode::MissionLanesCallback_( // FIXME: workaround to get the vehicle driving in autonomous mode until the // environment model is available if (msg_mission.ego_lane.centerline.size() == 0) { - // do not continue to publish empty trajectory + // Do not continue to publish empty trajectory if (mission_lanes_available_once_) { - // do only print warning if full mission lane was already available once + // Do only print warning if full mission lane was already available once RCLCPP_WARN(this->get_logger(), "Received empty ego mission lane, aborting conversion!"); } return; @@ -142,20 +142,21 @@ void MissionLaneConverterNode::MissionLanesCallback_( visualization_msgs::msg::Marker trj_vis_global = GetGlobalTrjVisualization_(trj_msg_global); vis_trajectory_publisher_global_->publish(trj_vis_global); - // publish trajectory to motion planner + // Publish trajectory to motion planner trajectory_publisher_->publish(trj_msg); trajectory_publisher_global_->publish(trj_msg_global); - // publish path to motion planner + // Publish path to motion planner path_publisher_->publish(path_msg); path_publisher_global_->publish(path_msg_global); // TODO(thomas.herrmann@driveblocks.ai): outsource this to a separate method - // clear all markers in scene + // Clear all markers in scene visualization_msgs::msg::Marker msg_marker; msg_marker.header = msg_mission.header; msg_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - // this specifies the clear all / delete all action + + // This specifies the clear all / delete all action msg_marker.action = 3; vis_trajectory_publisher_->publish(msg_marker); @@ -175,13 +176,13 @@ std::tuple< MissionLaneConverterNode::ConvertMissionToTrajectory( const autoware_planning_msgs::msg::MissionLanesStamped & msg) { - // empty trajectory for controller + // Empty trajectory for controller autoware_auto_planning_msgs::msg::Trajectory trj_msg = autoware_auto_planning_msgs::msg::Trajectory(); autoware_auto_planning_msgs::msg::Path path_msg = autoware_auto_planning_msgs::msg::Path(); - // empty trajectory visualization message + // Empty trajectory visualization message visualization_msgs::msg::Marker trj_vis, path_center_vis, path_left_vis, path_right_vis; visualization_msgs::msg::MarkerArray path_area_vis; @@ -191,12 +192,12 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( trj_vis.type = visualization_msgs::msg::Marker::LINE_STRIP; trj_vis.pose.orientation.w = 1.0; // Neutral orientation trj_vis.scale.x = 0.4; - trj_vis.color.g = 0.742; // green color - trj_vis.color.b = 0.703; // blue color + trj_vis.color.g = 0.742; // Green color + trj_vis.color.b = 0.703; // Blue color trj_vis.color.a = 0.750; - trj_vis.lifetime.sec = 0; // forever - trj_vis.lifetime.nanosec = 0; // forever - trj_vis.frame_locked = false; // always transform into baselink + trj_vis.lifetime.sec = 0; // Forever + trj_vis.lifetime.nanosec = 0; // Forever + trj_vis.frame_locked = false; // Always transform into baselink path_left_vis.header.frame_id = msg.header.frame_id; path_left_vis.header.stamp = msg.header.stamp; @@ -205,31 +206,30 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( path_left_vis.pose.orientation.w = 1.0; // Neutral orientation path_left_vis.scale.x = 0.6; - path_left_vis.color.g = 0.742; // green color - path_left_vis.color.b = 0.703; // blue color + path_left_vis.color.g = 0.742; // Green color + path_left_vis.color.b = 0.703; // Blue color path_left_vis.color.a = 0.350; - path_left_vis.lifetime.sec = 0; // forever - path_left_vis.lifetime.nanosec = 0; // forever - path_left_vis.frame_locked = false; // always transform into baselink + path_left_vis.lifetime.sec = 0; // Forever + path_left_vis.lifetime.nanosec = 0; // Forever + path_left_vis.frame_locked = false; // Always transform into baselink path_right_vis = path_left_vis; path_center_vis = path_left_vis; - // fill output trajectory - // header + // Fill output trajectory header trj_msg.header = msg.header; path_msg.header = msg.header; - // frame id must be "map" for Autoware controller + // Frame id must be "map" for Autoware controller trj_msg.header.frame_id = "map"; path_msg.header.frame_id = "map"; switch (msg.target_lane) { case 0: - // if target == 0, forward ego lane + // If target == 0, forward ego lane CreateMotionPlannerInput_( trj_msg, path_msg, trj_vis, path_center_vis, msg.ego_lane.centerline); - // fill path bounds left and right + // Fill path bounds left and right CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); break; @@ -238,40 +238,37 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( CreateMotionPlannerInput_( trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_left[0].centerline); - // fill path bounds left and right + // Fill path bounds left and right CreatePathBound_( path_msg.left_bound, path_left_vis, msg.drivable_lanes_left[0].bound_left, 1); CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); break; case 1: - // Lane change to the right CreateMotionPlannerInput_( trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right[0].centerline); - // fill path bounds left and right + // Fill path bounds left and right CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); CreatePathBound_( path_msg.right_bound, path_right_vis, msg.drivable_lanes_right[0].bound_right, 2); break; case -2: - - // take exit left + // Take exit left CreateMotionPlannerInput_( trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_left.back().centerline); - // fill path bounds left and right + // Fill path bounds left and right CreatePathBound_( path_msg.left_bound, path_left_vis, msg.drivable_lanes_left.back().bound_left, 1); CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); break; case 2: - - // take exit right + // Take exit right CreateMotionPlannerInput_( trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right.back().centerline); - // fill path bounds left and right + // Fill path bounds left and right CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); CreatePathBound_( path_msg.right_bound, path_right_vis, msg.drivable_lanes_right.back().bound_right, 2); @@ -299,11 +296,11 @@ void MissionLaneConverterNode::CreateMotionPlannerInput_( visualization_msgs::msg::Marker & path_vis, const std::vector & centerline_mission_lane) { - // add a mission lane's centerline to the output trajectory and path messages + // Add a mission lane's centerline to the output trajectory and path messages for (size_t idx_point = 0; idx_point < centerline_mission_lane.size(); idx_point++) { - // check if trajectory message is empty + // Check if trajectory message is empty if (trj_msg.points.size() > 0) { - // check if last trajectory point equals the one we want to add now + // Check if last trajectory point equals the one we want to add now if ( trj_msg.points.back().pose.position.x != centerline_mission_lane[idx_point].x && trj_msg.points.back().pose.position.y != centerline_mission_lane[idx_point].y) { @@ -311,26 +308,26 @@ void MissionLaneConverterNode::CreateMotionPlannerInput_( trj_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, target_speed_); - // similar point has to be added to the path message + // Similar point has to be added to the path message AddPathPoint_( path_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, target_speed_); - // add visualization marker to trajectory vis message + // Add visualization marker to trajectory vis message AddPointVisualizationMarker_( trj_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); - // add visualization marker to path vis message + // Add visualization marker to path vis message AddPointVisualizationMarker_( path_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); } - } else { // add first point - // add a trajectory point + } else { // Add first point + // Add a trajectory point AddTrajectoryPoint_( trj_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, target_speed_); - // similar point has to be added to the path message + // Similar point has to be added to the path message AddPathPoint_( path_msg, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, target_speed_); @@ -338,7 +335,7 @@ void MissionLaneConverterNode::CreateMotionPlannerInput_( AddPointVisualizationMarker_( trj_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); - // add visualization marker to path vis message + // Add visualization marker to path vis message AddPointVisualizationMarker_( path_vis, centerline_mission_lane[idx_point].x, centerline_mission_lane[idx_point].y, 0); } @@ -355,20 +352,20 @@ void MissionLaneConverterNode::CreatePathBound_( geometry_msgs::msg::Point pt_path; pt_path.x = bound_mission_lane[idx_point].x; pt_path.y = bound_mission_lane[idx_point].y; - // check if path bound is empty + // Check if path bound is empty if (bound_path.size() > 0) { - // check if last path point equals the one we want to add now + // Check if last path point equals the one we want to add now if ( bound_path.back().x != bound_mission_lane[idx_point].x && bound_path.back().y != bound_mission_lane[idx_point].y) { - // finally add the path point after successful checks + // Finally add the path point after successful checks bound_path.push_back(pt_path); } - } else { // add first point to path + } else { // Add first point to path bound_path.push_back(pt_path); } - // add last added point to a marker message for debugging + // Add last added point to a marker message for debugging // FIXME: probably no unique ids for multiple calls? AddPointVisualizationMarker_(path_vis, bound_path.back().x, bound_path.back().y, id_marker); } @@ -380,14 +377,14 @@ void MissionLaneConverterNode::AddPathPoint_( autoware_auto_planning_msgs::msg::Path & pth_msg, const double x, const double y, const double v_x) { - // add a trajectory point + // Add a trajectory point pth_msg.points.push_back(autoware_auto_planning_msgs::msg::PathPoint()); - // fill trajectory points with meaningful data + // Fill trajectory points with meaningful data pth_msg.points.back().pose.position.x = x; pth_msg.points.back().pose.position.y = y; - // constant velocity + // Constant velocity pth_msg.points.back().longitudinal_velocity_mps = v_x; return; @@ -397,14 +394,14 @@ void MissionLaneConverterNode::AddTrajectoryPoint_( autoware_auto_planning_msgs::msg::Trajectory & trj_msg, const double x, const double y, const double v_x) { - // add a trajectory point + // Add a trajectory point trj_msg.points.push_back(autoware_auto_planning_msgs::msg::TrajectoryPoint()); - // fill trajectory points with meaningful data + // Fill trajectory points with meaningful data trj_msg.points.back().pose.position.x = x; trj_msg.points.back().pose.position.y = y; - // constant velocity + // Constant velocity trj_msg.points.back().longitudinal_velocity_mps = v_x; return; @@ -413,7 +410,7 @@ void MissionLaneConverterNode::AddTrajectoryPoint_( void MissionLaneConverterNode::AddPointVisualizationMarker_( visualization_msgs::msg::Marker & trj_vis, const double x, const double y, const int id_marker) { - // fill visualization message + // Fill visualization message trj_vis.points.push_back(geometry_msgs::msg::Point()); trj_vis.points.back().x = x; @@ -434,7 +431,7 @@ void MissionLaneConverterNode::AddHeadingToTrajectory_( points.back().y = trj_msg.points[idx_point].pose.position.y; } - // only execute if we have at least 2 points + // Only execute if we have at least 2 points if (points.size() > 1) { std::vector psi_vec = GetPsiForPoints(points); @@ -456,7 +453,7 @@ void MissionLaneConverterNode::AddHeadingToTrajectory_( // the output conversion void MissionLaneConverterNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg) { - // store current odometry information + // Store current odometry information last_odom_msg_ = msg; if (!received_motion_update_once_) { @@ -472,12 +469,12 @@ void MissionLaneConverterNode::CallbackOdometryMessages_(const nav_msgs::msg::Od odom_vis.type = visualization_msgs::msg::Marker::POINTS; odom_vis.pose.orientation.w = 1.0; // Neutral orientation odom_vis.scale.x = 5.0; - odom_vis.color.g = 0.0; // green color - odom_vis.color.b = 0.7; // blue color + odom_vis.color.g = 0.0; // Green color + odom_vis.color.b = 0.7; // Blue color odom_vis.color.a = 0.7; - odom_vis.lifetime.sec = 0; // forever - odom_vis.lifetime.nanosec = 0; // forever - odom_vis.frame_locked = false; // always transform into baselink + odom_vis.lifetime.sec = 0; // Forever + odom_vis.lifetime.nanosec = 0; // Forever + odom_vis.frame_locked = false; // Always transform into baselink odom_vis.points.push_back(geometry_msgs::msg::Point()); @@ -493,7 +490,7 @@ void MissionLaneConverterNode::CallbackOdometryMessages_(const nav_msgs::msg::Od template T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) { - // define output message + // Define output message T msg_output = msg_input; msg_output.header.frame_id = "map"; @@ -533,10 +530,10 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) const Pose2D pose_cur( last_odom_msg_.pose.pose.position.x, last_odom_msg_.pose.pose.position.y, psi_cur); - // get relationship from current odom frame to global map frame origin + // Get relationship from current odom frame to global map frame origin const Pose2D d_current_to_map_origin = TransformToNewCosy2D(pose_cur, Pose2D{0.0, 0.0}); - // convert all the input points to the global map frame + // Convert all the input points to the global map frame for (size_t i = 0; i < msg_input.points.size(); i++) { // convert input pose const double psi_point = GetYawFromQuaternion( @@ -545,14 +542,14 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) const Pose2D pose( msg_input.points[i].pose.position.x, msg_input.points[i].pose.position.y, psi_point); - // express point in global map frame + // Express point in global map frame Pose2D pose_map = TransformToNewCosy2D(d_current_to_map_origin, pose); msg_output.points[i].pose.position.x = pose_map.get_x(); msg_output.points[i].pose.position.y = pose_map.get_y(); } - // convert the path area's bounds + // Convert the path area's bounds if constexpr (std::is_same::value) { for (size_t ib = 0; ib < 2; ib++) { std::vector bound; @@ -561,10 +558,10 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) else bound = msg_input.right_bound; for (size_t i = 0; i < bound.size(); i++) { - // convert input pose + // Convert input pose const Pose2D pose(bound[i].x, bound[i].y); - // express point in global map frame + // Express point in global map frame Pose2D pose_map = TransformToNewCosy2D(d_current_to_map_origin, pose); if (ib == 0) { @@ -578,7 +575,7 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) } } - // add heading if type is a trajectory + // Add heading if type is a trajectory if constexpr (std::is_same::value) AddHeadingToTrajectory_(msg_output); } @@ -589,7 +586,7 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) visualization_msgs::msg::Marker MissionLaneConverterNode::GetGlobalTrjVisualization_( const autoware_auto_planning_msgs::msg::Trajectory & trj_msg) { - // empty trajectory visualization message + // Empty trajectory visualization message visualization_msgs::msg::Marker trj_vis_global; trj_vis_global.header.frame_id = trj_msg.header.frame_id; trj_vis_global.header.stamp = trj_msg.header.stamp; @@ -597,12 +594,12 @@ visualization_msgs::msg::Marker MissionLaneConverterNode::GetGlobalTrjVisualizat trj_vis_global.type = visualization_msgs::msg::Marker::LINE_STRIP; trj_vis_global.pose.orientation.w = 1.0; // Neutral orientation trj_vis_global.scale.x = 0.4; - trj_vis_global.color.g = 0.0; // green color - trj_vis_global.color.b = 0.703; // blue color + trj_vis_global.color.g = 0.0; // Green color + trj_vis_global.color.b = 0.703; // Blue color trj_vis_global.color.a = 0.0; - trj_vis_global.lifetime.sec = 0; // forever - trj_vis_global.lifetime.nanosec = 0; // forever - trj_vis_global.frame_locked = false; // always transform into baselink + trj_vis_global.lifetime.sec = 0; // Forever + trj_vis_global.lifetime.nanosec = 0; // Forever + trj_vis_global.frame_locked = false; // Always transform into baselink for (size_t i = 0; i < trj_msg.points.size(); i++) { AddPointVisualizationMarker_( diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp index 5d427c217a3d6..78aef5e313ccf 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp @@ -7,8 +7,13 @@ namespace autoware::mapless_architecture { +/** + * @brief Test the ConvertMissionToTrajectory() function. + * + * @return int: returns 0 on success + */ int TestMissionToTrajectory(); -} +} // namespace autoware::mapless_architecture #endif // TEST_MISSION_PLANNER_CONVERTER_HPP_ diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp index 33af0b6f754ec..51d7178e445aa 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -15,24 +15,24 @@ int TestMissionToTrajectory() autoware_planning_msgs::msg::MissionLanesStamped mission_msg; - // set target lane to ego lane + // Set target lane to ego lane mission_msg.target_lane = 0; - // add a driving corridor to the ego lane + // Add a driving corridor to the ego lane mission_msg.ego_lane = autoware_planning_msgs::msg::DrivingCorridor(); - // add points to the ego lane centerline + // Add points to the ego lane centerline mission_msg.ego_lane.centerline.push_back(geometry_msgs::msg::Point()); mission_msg.ego_lane.centerline.back().x = 0.0; mission_msg.ego_lane.centerline.back().y = 0.0; - // get converted trajectory + // Get converted trajectory std::tuple< autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); - // extract trajectory + // Extract trajectory autoware_auto_planning_msgs::msg::Trajectory trj_msg = std::get<0>(mission_to_trj); EXPECT_EQ(trj_msg.points.back().pose.position.x, mission_msg.ego_lane.centerline.back().x); @@ -51,10 +51,10 @@ int TestMissionToTrajectory() mission_msg.ego_lane.centerline.back().x = 2.0; mission_msg.ego_lane.centerline.back().y = 2.0; - // convert + // Convert mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); - // extract trajectory + // Extract trajectory trj_msg = std::get<0>(mission_to_trj); EXPECT_EQ(trj_msg.points.back().pose.position.x, mission_msg.ego_lane.centerline.back().x); @@ -69,13 +69,13 @@ int TestMissionToTrajectory() mission_msg.drivable_lanes_left.back().centerline.back().x = 5.0; mission_msg.drivable_lanes_left.back().centerline.back().y = 3.0; - // set target lane to neighbor left + // Set target lane to neighbor left mission_msg.target_lane = -1; - // convert + // Convert mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); - // extract trajectory + // Extract trajectory trj_msg = std::get<0>(mission_to_trj); EXPECT_EQ( @@ -94,13 +94,13 @@ int TestMissionToTrajectory() mission_msg.drivable_lanes_right.back().centerline.back().x = 3.0; mission_msg.drivable_lanes_right.back().centerline.back().y = 3.6; - // set target lane to neighbor right + // Set target lane to neighbor right mission_msg.target_lane = 1; - // convert + // Convert mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); - // extract trajectory + // Extract trajectory trj_msg = std::get<0>(mission_to_trj); EXPECT_EQ( From b048568c17d97cf187db7c14860c1fc0fbfe3694 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Tue, 25 Jun 2024 11:50:40 +0000 Subject: [PATCH 14/24] Fix bug Signed-off-by: Simon Eisenmann --- .../test/src/test_mission_planner_core.cpp | 10 +++++----- .../src/local_road_provider_node.cpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index eacb9914a6edf..2884017d40470 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -167,7 +167,7 @@ int TestGetPointOnLane() // Convert message autoware_planning_msgs::msg::RoadSegments road_segments = - lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg_lanelets); + ConvertLaneletsStamped2RoadSegments(msg_lanelets); MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); @@ -205,7 +205,7 @@ int TestIsOnGoalLane() // Convert message autoware_planning_msgs::msg::RoadSegments road_segments = - lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg_road_model); + ConvertLaneletsStamped2RoadSegments(msg_road_model); MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); @@ -330,7 +330,7 @@ int TestRecenterGoalpoint() // Convert message autoware_planning_msgs::msg::RoadSegments road_segments = - lib_mission_planner::ConvertLaneletsStamped2RoadSegments(local_road_model); + ConvertLaneletsStamped2RoadSegments(local_road_model); mission_planner.ConvertInput2LaneletFormat( road_segments, converted_lanelets, lanelet_connections); @@ -396,7 +396,7 @@ int TestCheckIfGoalPointShouldBeReset() // Convert message autoware_planning_msgs::msg::RoadSegments road_segments = - lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg); + ConvertLaneletsStamped2RoadSegments(msg); autoware_planning_msgs::msg::LocalMap local_map; local_map.road_segments = road_segments; @@ -542,7 +542,7 @@ std::tuple, std::vector> Create // Convert message autoware_planning_msgs::msg::RoadSegments road_segments = - lib_mission_planner::ConvertLaneletsStamped2RoadSegments(message); + ConvertLaneletsStamped2RoadSegments(message); MissionPlanner.ConvertInput2LaneletFormat(road_segments, converted_lanelets, lanelet_connections); return std::make_tuple(converted_lanelets, lanelet_connections); diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp index f4fb5b469cfe4..098ff6f03c447 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp @@ -31,7 +31,7 @@ LocalRoadProviderNode::LocalRoadProviderNode() : Node("local_road_provider_node" void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg) { autoware_planning_msgs::msg::RoadSegments road_segments = - lib_mission_planner::ConvertLaneletsStamped2RoadSegments(msg); + ConvertLaneletsStamped2RoadSegments(msg); // Publish the RoadSegments message road_publisher_->publish(road_segments); From 60bff85ce93ef31667e05e91b60346b44e33f238 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Wed, 26 Jun 2024 07:52:11 +0000 Subject: [PATCH 15/24] Remove driveblocks dependencies Signed-off-by: Simon Eisenmann --- .../mission_planner_node.hpp | 1 - .../src/mission_planner_node.cpp | 1 - .../test/src/test_mission_planner_core.cpp | 356 +++++++----------- .../helper_functions.hpp | 10 - .../src/helper_functions.cpp | 49 --- .../local_road_provider_node.hpp | 9 + .../src/local_road_provider_node.cpp | 50 +++ 7 files changed, 196 insertions(+), 280 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 7688c6016ba14..403e6d834b811 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -14,7 +14,6 @@ #include "autoware_planning_msgs/msg/mission.hpp" #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" #include "autoware_planning_msgs/msg/visualization_distance.hpp" -#include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index fed74e3e0a9fd..6183b866b1866 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -11,7 +11,6 @@ #include "autoware_planning_msgs/msg/driving_corridor.hpp" #include "autoware_planning_msgs/msg/mission.hpp" #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" -#include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "visualization_msgs/msg/marker.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index 2884017d40470..fe74b58bd88df 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -5,7 +5,6 @@ #include "autoware/local_mission_planner_common/helper_functions.hpp" #include "gtest/gtest.h" -#include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -13,49 +12,25 @@ namespace autoware::mapless_architecture { +// Note: Lanelets and Segments are basically the same! -db_msgs::msg::LaneletsStamped CreateLanelets() +autoware_planning_msgs::msg::RoadSegments CreateSegments() { // Local variables - const int n_lanelets = 3; - const int n_attribute_vecs = 1; - const int n_points = 2; + const int n_segments = 3; // Fill lanelet2 message - db_msgs::msg::LaneletsStamped message; - std::vector lanelet_vec(n_lanelets); - message.lanelets = lanelet_vec; + autoware_planning_msgs::msg::RoadSegments message; + std::vector lanelet_vec(n_segments); + message.segments = lanelet_vec; // Global position geometry_msgs::msg::PoseStamped msg_global_pose; msg_global_pose.header.frame_id = "base_link"; - std::array linestring_vec; - std::vector attribute_vec(n_attribute_vecs); - message.lanelets[0].id = 0; - message.lanelets[0].linestrings = linestring_vec; - message.lanelets[0].attributes = attribute_vec; - message.lanelets[1].id = 1; - message.lanelets[1].linestrings = linestring_vec; - message.lanelets[1].attributes = attribute_vec; - message.lanelets[2].id = 2; - message.lanelets[2].linestrings = linestring_vec; - message.lanelets[2].attributes = attribute_vec; - - uint16_t id = 0; - std::vector points_vec(n_points); - message.lanelets[0].linestrings[0].id = id; - message.lanelets[0].linestrings[1].id = ++id; - message.lanelets[0].linestrings[0].points = points_vec; - message.lanelets[0].linestrings[1].points = points_vec; - message.lanelets[1].linestrings[0].id = ++id; - message.lanelets[1].linestrings[1].id = ++id; - message.lanelets[1].linestrings[0].points = points_vec; - message.lanelets[1].linestrings[1].points = points_vec; - message.lanelets[2].linestrings[0].id = ++id; - message.lanelets[2].linestrings[1].id = ++id; - message.lanelets[2].linestrings[0].points = points_vec; - message.lanelets[2].linestrings[1].points = points_vec; + message.segments[0].id = 0; + message.segments[1].id = 1; + message.segments[2].id = 2; tf2::Quaternion quaternion; @@ -70,55 +45,55 @@ db_msgs::msg::LaneletsStamped CreateLanelets() // Street layout in current local cosy = global cosy since vehicle position is // at the origin in the global cosy - message.lanelets[0].linestrings[0].points[0].pose.position.x = -2.0; - message.lanelets[0].linestrings[0].points[0].pose.position.y = -0.5; - message.lanelets[0].linestrings[0].points[0].pose.position.z = 0.0; - message.lanelets[0].linestrings[0].points[1].pose.position.x = 10.0; - message.lanelets[0].linestrings[0].points[1].pose.position.y = -0.5; - message.lanelets[0].linestrings[0].points[1].pose.position.z = 0.0; - - message.lanelets[0].linestrings[1].points[0].pose.position.x = -2.0; - message.lanelets[0].linestrings[1].points[0].pose.position.y = 0.5; - message.lanelets[0].linestrings[1].points[0].pose.position.z = 0.0; - message.lanelets[0].linestrings[1].points[1].pose.position.x = 10.0; - message.lanelets[0].linestrings[1].points[1].pose.position.y = 0.5; - message.lanelets[0].linestrings[1].points[1].pose.position.z = 0.0; - - message.lanelets[1].linestrings[0].points[0].pose.position.x = 0.0; - message.lanelets[1].linestrings[0].points[0].pose.position.y = 1.0; - message.lanelets[1].linestrings[0].points[0].pose.position.z = 0.0; - message.lanelets[1].linestrings[0].points[1].pose.position.x = 10.0; - message.lanelets[1].linestrings[0].points[1].pose.position.y = 1.0; - message.lanelets[1].linestrings[0].points[1].pose.position.z = 0.0; - - message.lanelets[1].linestrings[1].points[0].pose.position.x = 0.0; - message.lanelets[1].linestrings[1].points[0].pose.position.y = 2.0; - message.lanelets[1].linestrings[1].points[0].pose.position.z = 0.0; - message.lanelets[1].linestrings[1].points[1].pose.position.x = 10.0; - message.lanelets[1].linestrings[1].points[1].pose.position.y = 2.0; - message.lanelets[1].linestrings[1].points[1].pose.position.z = 0.0; - - message.lanelets[2].linestrings[0].points[0].pose.position.x = 10.0; - message.lanelets[2].linestrings[0].points[0].pose.position.y = -0.5; - message.lanelets[2].linestrings[0].points[0].pose.position.z = 0.0; - message.lanelets[2].linestrings[0].points[1].pose.position.x = 20.0; - message.lanelets[2].linestrings[0].points[1].pose.position.y = -0.5; - message.lanelets[2].linestrings[0].points[1].pose.position.z = 0.0; - - message.lanelets[2].linestrings[1].points[0].pose.position.x = 10.0; - message.lanelets[2].linestrings[1].points[0].pose.position.y = 0.5; - message.lanelets[2].linestrings[1].points[0].pose.position.z = 0.0; - message.lanelets[2].linestrings[1].points[1].pose.position.x = 20.0; - message.lanelets[2].linestrings[1].points[1].pose.position.y = 0.5; - message.lanelets[2].linestrings[1].points[1].pose.position.z = 0.0; + message.segments[0].linestrings[0].poses[0].position.x = -2.0; + message.segments[0].linestrings[0].poses[0].position.y = -0.5; + message.segments[0].linestrings[0].poses[0].position.z = 0.0; + message.segments[0].linestrings[0].poses[1].position.x = 10.0; + message.segments[0].linestrings[0].poses[1].position.y = -0.5; + message.segments[0].linestrings[0].poses[1].position.z = 0.0; + + message.segments[0].linestrings[1].poses[0].position.x = -2.0; + message.segments[0].linestrings[1].poses[0].position.y = 0.5; + message.segments[0].linestrings[1].poses[0].position.z = 0.0; + message.segments[0].linestrings[1].poses[1].position.x = 10.0; + message.segments[0].linestrings[1].poses[1].position.y = 0.5; + message.segments[0].linestrings[1].poses[1].position.z = 0.0; + + message.segments[1].linestrings[0].poses[0].position.x = 0.0; + message.segments[1].linestrings[0].poses[0].position.y = 1.0; + message.segments[1].linestrings[0].poses[0].position.z = 0.0; + message.segments[1].linestrings[0].poses[1].position.x = 10.0; + message.segments[1].linestrings[0].poses[1].position.y = 1.0; + message.segments[1].linestrings[0].poses[1].position.z = 0.0; + + message.segments[1].linestrings[1].poses[0].position.x = 0.0; + message.segments[1].linestrings[1].poses[0].position.y = 2.0; + message.segments[1].linestrings[1].poses[0].position.z = 0.0; + message.segments[1].linestrings[1].poses[1].position.x = 10.0; + message.segments[1].linestrings[1].poses[1].position.y = 2.0; + message.segments[1].linestrings[1].poses[1].position.z = 0.0; + + message.segments[2].linestrings[0].poses[0].position.x = 10.0; + message.segments[2].linestrings[0].poses[0].position.y = -0.5; + message.segments[2].linestrings[0].poses[0].position.z = 0.0; + message.segments[2].linestrings[0].poses[1].position.x = 20.0; + message.segments[2].linestrings[0].poses[1].position.y = -0.5; + message.segments[2].linestrings[0].poses[1].position.z = 0.0; + + message.segments[2].linestrings[1].poses[0].position.x = 10.0; + message.segments[2].linestrings[1].poses[0].position.y = 0.5; + message.segments[2].linestrings[1].poses[0].position.z = 0.0; + message.segments[2].linestrings[1].poses[1].position.x = 20.0; + message.segments[2].linestrings[1].poses[1].position.y = 0.5; + message.segments[2].linestrings[1].poses[1].position.z = 0.0; // Define connections - message.lanelets[0].neighboring_lanelet_id = {0, 1}; - message.lanelets[1].neighboring_lanelet_id = {0, 1}; - message.lanelets[2].neighboring_lanelet_id = {-1, -1}; - message.lanelets[0].successor_lanelet_id = {2}; - message.lanelets[1].successor_lanelet_id = {-1}; - message.lanelets[2].successor_lanelet_id = {-1}; + message.segments[0].neighboring_lanelet_id = {0, 1}; + message.segments[1].neighboring_lanelet_id = {0, 1}; + message.segments[2].neighboring_lanelet_id = {-1, -1}; + message.segments[0].successor_lanelet_id = {2}; + message.segments[1].successor_lanelet_id = {-1}; + message.segments[2].successor_lanelet_id = {-1}; return message; } @@ -155,8 +130,8 @@ int TestCalculateDistanceBetweenPointAndLineString() int TestGetPointOnLane() { - // Create some example lanelets - auto msg_lanelets = CreateLanelets(); + // Create some example segments + autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); @@ -165,10 +140,6 @@ int TestGetPointOnLane() std::vector lanelet_connections; std::vector lanelets; - // Convert message - autoware_planning_msgs::msg::RoadSegments road_segments = - ConvertLaneletsStamped2RoadSegments(msg_lanelets); - MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); // Get a point from the tested function which has the x value 3.0 and lies on @@ -193,8 +164,8 @@ int TestGetPointOnLane() int TestIsOnGoalLane() { - // Create some example lanelets - auto msg_road_model = CreateLanelets(); + // Create some example segments + autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); @@ -203,10 +174,6 @@ int TestIsOnGoalLane() std::vector lanelet_connections; std::vector lanelets; - // Convert message - autoware_planning_msgs::msg::RoadSegments road_segments = - ConvertLaneletsStamped2RoadSegments(msg_road_model); - MissionPlanner.ConvertInput2LaneletFormat(road_segments, lanelets, lanelet_connections); // Define a point with x = 1.0 and y = 0.0 @@ -230,41 +197,22 @@ int TestIsOnGoalLane() return 0; } -db_msgs::msg::LaneletsStamped GetTestRoadModelForRecenterTests() +autoware_planning_msgs::msg::RoadSegments GetTestRoadModelForRecenterTests() { // local variables - const int n_lanelets = 2; - const int n_attribute_vecs = 1; - const int n_points = 2; + const int n_segments = 2; // Fill lanelet2 message - db_msgs::msg::LaneletsStamped message; - std::vector lanelet_vec(n_lanelets); - message.lanelets = lanelet_vec; + autoware_planning_msgs::msg::RoadSegments message; + std::vector lanelet_vec(n_segments); + message.segments = lanelet_vec; // Global position geometry_msgs::msg::PoseStamped msg_global_pose; msg_global_pose.header.frame_id = "base_link"; - std::array linestring_vec; - std::vector attribute_vec(n_attribute_vecs); - message.lanelets[0].id = 0; - message.lanelets[0].linestrings = linestring_vec; - message.lanelets[0].attributes = attribute_vec; - message.lanelets[1].id = 1; - message.lanelets[1].linestrings = linestring_vec; - message.lanelets[1].attributes = attribute_vec; - - uint16_t id = 0; - std::vector points_vec(n_points); - message.lanelets[0].linestrings[0].id = id; - message.lanelets[0].linestrings[1].id = ++id; - message.lanelets[0].linestrings[0].points = points_vec; - message.lanelets[0].linestrings[1].points = points_vec; - message.lanelets[1].linestrings[0].id = ++id; - message.lanelets[1].linestrings[1].id = ++id; - message.lanelets[1].linestrings[0].points = points_vec; - message.lanelets[1].linestrings[1].points = points_vec; + message.segments[0].id = 0; + message.segments[1].id = 1; tf2::Quaternion quaternion; @@ -279,39 +227,39 @@ db_msgs::msg::LaneletsStamped GetTestRoadModelForRecenterTests() // Street layout in current local cosy = global cosy since vehicle position is // at the origin in the global cosy - message.lanelets[0].linestrings[0].points[0].pose.position.x = 0.0; - message.lanelets[0].linestrings[0].points[0].pose.position.y = -0.5; - message.lanelets[0].linestrings[0].points[0].pose.position.z = 0.0; - message.lanelets[0].linestrings[0].points[1].pose.position.x = 10.0; - message.lanelets[0].linestrings[0].points[1].pose.position.y = -0.5; - message.lanelets[0].linestrings[0].points[1].pose.position.z = 0.0; - - message.lanelets[0].linestrings[1].points[0].pose.position.x = 0.0; - message.lanelets[0].linestrings[1].points[0].pose.position.y = 0.5; - message.lanelets[0].linestrings[1].points[0].pose.position.z = 0.0; - message.lanelets[0].linestrings[1].points[1].pose.position.x = 10.0; - message.lanelets[0].linestrings[1].points[1].pose.position.y = 0.5; - message.lanelets[0].linestrings[1].points[1].pose.position.z = 0.0; - - message.lanelets[1].linestrings[0].points[0].pose.position.x = 0.0; - message.lanelets[1].linestrings[0].points[0].pose.position.y = 1.0; - message.lanelets[1].linestrings[0].points[0].pose.position.z = 0.0; - message.lanelets[1].linestrings[0].points[1].pose.position.x = 10.0; - message.lanelets[1].linestrings[0].points[1].pose.position.y = 1.0; - message.lanelets[1].linestrings[0].points[1].pose.position.z = 0.0; - - message.lanelets[1].linestrings[1].points[0].pose.position.x = 0.0; - message.lanelets[1].linestrings[1].points[0].pose.position.y = 2.0; - message.lanelets[1].linestrings[1].points[0].pose.position.z = 0.0; - message.lanelets[1].linestrings[1].points[1].pose.position.x = 10.0; - message.lanelets[1].linestrings[1].points[1].pose.position.y = 2.0; - message.lanelets[1].linestrings[1].points[1].pose.position.z = 0.0; + message.segments[0].linestrings[0].poses[0].position.x = 0.0; + message.segments[0].linestrings[0].poses[0].position.y = -0.5; + message.segments[0].linestrings[0].poses[0].position.z = 0.0; + message.segments[0].linestrings[0].poses[1].position.x = 10.0; + message.segments[0].linestrings[0].poses[1].position.y = -0.5; + message.segments[0].linestrings[0].poses[1].position.z = 0.0; + + message.segments[0].linestrings[1].poses[0].position.x = 0.0; + message.segments[0].linestrings[1].poses[0].position.y = 0.5; + message.segments[0].linestrings[1].poses[0].position.z = 0.0; + message.segments[0].linestrings[1].poses[1].position.x = 10.0; + message.segments[0].linestrings[1].poses[1].position.y = 0.5; + message.segments[0].linestrings[1].poses[1].position.z = 0.0; + + message.segments[1].linestrings[0].poses[0].position.x = 0.0; + message.segments[1].linestrings[0].poses[0].position.y = 1.0; + message.segments[1].linestrings[0].poses[0].position.z = 0.0; + message.segments[1].linestrings[0].poses[1].position.x = 10.0; + message.segments[1].linestrings[0].poses[1].position.y = 1.0; + message.segments[1].linestrings[0].poses[1].position.z = 0.0; + + message.segments[1].linestrings[1].poses[0].position.x = 0.0; + message.segments[1].linestrings[1].poses[0].position.y = 2.0; + message.segments[1].linestrings[1].poses[0].position.z = 0.0; + message.segments[1].linestrings[1].poses[1].position.x = 10.0; + message.segments[1].linestrings[1].poses[1].position.y = 2.0; + message.segments[1].linestrings[1].poses[1].position.z = 0.0; int32_t neigh_1 = 1; std::vector neighboring_ids = {neigh_1}; - message.lanelets[0].neighboring_lanelet_id = neighboring_ids; + message.segments[0].neighboring_lanelet_id = neighboring_ids; std::vector neighboring_ids_2 = {}; - message.lanelets[1].neighboring_lanelet_id = neighboring_ids_2; + message.segments[1].neighboring_lanelet_id = neighboring_ids_2; return message; } @@ -322,16 +270,12 @@ int TestRecenterGoalpoint() MissionPlannerNode mission_planner = MissionPlannerNode(); // Get a local road model for testing - db_msgs::msg::LaneletsStamped local_road_model = GetTestRoadModelForRecenterTests(); + autoware_planning_msgs::msg::RoadSegments road_segments = GetTestRoadModelForRecenterTests(); // Used for the output std::vector lanelet_connections; std::vector converted_lanelets; - // Convert message - autoware_planning_msgs::msg::RoadSegments road_segments = - ConvertLaneletsStamped2RoadSegments(local_road_model); - mission_planner.ConvertInput2LaneletFormat( road_segments, converted_lanelets, lanelet_connections); @@ -391,12 +335,9 @@ int TestRecenterGoalpoint() int TestCheckIfGoalPointShouldBeReset() { - // Create some example lanelets - auto msg = CreateLanelets(); + // Create some example segments + autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); - // Convert message - autoware_planning_msgs::msg::RoadSegments road_segments = - ConvertLaneletsStamped2RoadSegments(msg); autoware_planning_msgs::msg::LocalMap local_map; local_map.road_segments = road_segments; @@ -453,38 +394,19 @@ int TestCheckIfGoalPointShouldBeReset() std::tuple, std::vector> CreateLane() { // Local variables - const int n_lanelets = 2; - const int n_attribute_vecs = 1; - const int n_points = 2; + const int n_segments = 2; // Fill lanelet2 message - db_msgs::msg::LaneletsStamped message; - std::vector lanelet_vec(n_lanelets); - message.lanelets = lanelet_vec; + autoware_planning_msgs::msg::RoadSegments message; + std::vector lanelet_vec(n_segments); + message.segments = lanelet_vec; // Global position geometry_msgs::msg::PoseStamped msg_global_pose; msg_global_pose.header.frame_id = "base_link"; - std::array linestring_vec; - std::vector attribute_vec(n_attribute_vecs); - message.lanelets[0].id = 0; - message.lanelets[0].linestrings = linestring_vec; - message.lanelets[0].attributes = attribute_vec; - message.lanelets[1].id = 1; - message.lanelets[1].linestrings = linestring_vec; - message.lanelets[1].attributes = attribute_vec; - - uint16_t id = 0; - std::vector points_vec(n_points); - message.lanelets[0].linestrings[0].id = id; - message.lanelets[0].linestrings[1].id = ++id; - message.lanelets[0].linestrings[0].points = points_vec; - message.lanelets[0].linestrings[1].points = points_vec; - message.lanelets[1].linestrings[0].id = ++id; - message.lanelets[1].linestrings[1].id = ++id; - message.lanelets[1].linestrings[0].points = points_vec; - message.lanelets[1].linestrings[1].points = points_vec; + message.segments[0].id = 0; + message.segments[1].id = 1; tf2::Quaternion quaternion; @@ -499,39 +421,39 @@ std::tuple, std::vector> Create // Street layout in current local cosy = global cosy since vehicle position is // at the origin in the global cosy - message.lanelets[0].linestrings[0].points[0].pose.position.x = -2.0; - message.lanelets[0].linestrings[0].points[0].pose.position.y = -0.5; - message.lanelets[0].linestrings[0].points[0].pose.position.z = 0.0; - message.lanelets[0].linestrings[0].points[1].pose.position.x = 10.0; - message.lanelets[0].linestrings[0].points[1].pose.position.y = -0.5; - message.lanelets[0].linestrings[0].points[1].pose.position.z = 0.0; - - message.lanelets[0].linestrings[1].points[0].pose.position.x = -2.0; - message.lanelets[0].linestrings[1].points[0].pose.position.y = 0.5; - message.lanelets[0].linestrings[1].points[0].pose.position.z = 0.0; - message.lanelets[0].linestrings[1].points[1].pose.position.x = 10.0; - message.lanelets[0].linestrings[1].points[1].pose.position.y = 0.5; - message.lanelets[0].linestrings[1].points[1].pose.position.z = 0.0; - - message.lanelets[1].linestrings[0].points[0].pose.position.x = 10.0; - message.lanelets[1].linestrings[0].points[0].pose.position.y = -0.5; - message.lanelets[1].linestrings[0].points[0].pose.position.z = 0.0; - message.lanelets[1].linestrings[0].points[1].pose.position.x = 20.0; - message.lanelets[1].linestrings[0].points[1].pose.position.y = -0.5; - message.lanelets[1].linestrings[0].points[1].pose.position.z = 0.0; - - message.lanelets[1].linestrings[1].points[0].pose.position.x = 10.0; - message.lanelets[1].linestrings[1].points[0].pose.position.y = 0.5; - message.lanelets[1].linestrings[1].points[0].pose.position.z = 0.0; - message.lanelets[1].linestrings[1].points[1].pose.position.x = 20.0; - message.lanelets[1].linestrings[1].points[1].pose.position.y = 0.5; - message.lanelets[1].linestrings[1].points[1].pose.position.z = 0.0; + message.segments[0].linestrings[0].poses[0].position.x = -2.0; + message.segments[0].linestrings[0].poses[0].position.y = -0.5; + message.segments[0].linestrings[0].poses[0].position.z = 0.0; + message.segments[0].linestrings[0].poses[1].position.x = 10.0; + message.segments[0].linestrings[0].poses[1].position.y = -0.5; + message.segments[0].linestrings[0].poses[1].position.z = 0.0; + + message.segments[0].linestrings[1].poses[0].position.x = -2.0; + message.segments[0].linestrings[1].poses[0].position.y = 0.5; + message.segments[0].linestrings[1].poses[0].position.z = 0.0; + message.segments[0].linestrings[1].poses[1].position.x = 10.0; + message.segments[0].linestrings[1].poses[1].position.y = 0.5; + message.segments[0].linestrings[1].poses[1].position.z = 0.0; + + message.segments[1].linestrings[0].poses[0].position.x = 10.0; + message.segments[1].linestrings[0].poses[0].position.y = -0.5; + message.segments[1].linestrings[0].poses[0].position.z = 0.0; + message.segments[1].linestrings[0].poses[1].position.x = 20.0; + message.segments[1].linestrings[0].poses[1].position.y = -0.5; + message.segments[1].linestrings[0].poses[1].position.z = 0.0; + + message.segments[1].linestrings[1].poses[0].position.x = 10.0; + message.segments[1].linestrings[1].poses[0].position.y = 0.5; + message.segments[1].linestrings[1].poses[0].position.z = 0.0; + message.segments[1].linestrings[1].poses[1].position.x = 20.0; + message.segments[1].linestrings[1].poses[1].position.y = 0.5; + message.segments[1].linestrings[1].poses[1].position.z = 0.0; // Define connections - message.lanelets[0].successor_lanelet_id = {1}; - message.lanelets[0].neighboring_lanelet_id = {-1, -1}; - message.lanelets[1].successor_lanelet_id = {-1}; - message.lanelets[1].neighboring_lanelet_id = {-1, -1}; + message.segments[0].successor_lanelet_id = {1}; + message.segments[0].neighboring_lanelet_id = {-1, -1}; + message.segments[1].successor_lanelet_id = {-1}; + message.segments[1].neighboring_lanelet_id = {-1, -1}; // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); @@ -540,11 +462,7 @@ std::tuple, std::vector> Create std::vector lanelet_connections; std::vector converted_lanelets; - // Convert message - autoware_planning_msgs::msg::RoadSegments road_segments = - ConvertLaneletsStamped2RoadSegments(message); - - MissionPlanner.ConvertInput2LaneletFormat(road_segments, converted_lanelets, lanelet_connections); + MissionPlanner.ConvertInput2LaneletFormat(message, converted_lanelets, lanelet_connections); return std::make_tuple(converted_lanelets, lanelet_connections); } diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index ce86ea0377da7..7fb72b92ae249 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -10,7 +10,6 @@ #include "tf2_ros/transform_listener.h" #include "autoware_planning_msgs/msg/road_segments.hpp" -#include "db_msgs/msg/lanelets_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -92,15 +91,6 @@ double NormalizePsi(const double psi); */ std::vector GetPsiForPoints(const std::vector & points); -/** - * @brief Convert the LaneletsStamped message into a RoadSegments message. - * - * @param msg The message (db_msgs::msg::LaneletsStamped). - * @return autoware_planning_msgs::msg::RoadSegments. - */ -autoware_planning_msgs::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( - const db_msgs::msg::LaneletsStamped & msg); - /** * @brief Holds * the origin lanelet Id, diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index b65bc90c264f1..37e77b4ecaff1 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -6,7 +6,6 @@ #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/road_segments.hpp" -#include "db_msgs/msg/lanelets_stamped.hpp" namespace autoware::mapless_architecture { @@ -140,54 +139,6 @@ double GetYawFromQuaternion(const double x, const double y, const double z, cons return yaw; } -// Declare a static logger -static rclcpp::Logger static_logger = rclcpp::get_logger("static_logger"); - -autoware_planning_msgs::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( - const db_msgs::msg::LaneletsStamped & msg) -{ - // Initialize road segments message - autoware_planning_msgs::msg::RoadSegments road_segments; - - // Fill message header and pose - road_segments.header = msg.header; - road_segments.pose = msg.pose; - - // Convert lanelets to segments if lanelets are not empty - if (!msg.lanelets.empty()) { - for (const db_msgs::msg::Lanelet & lanelet : msg.lanelets) { - // Initialize a segment - autoware_planning_msgs::msg::Segment segment; - - // Fill the segment with basic information - segment.id = lanelet.id; - segment.successor_lanelet_id = lanelet.successor_lanelet_id; - segment.neighboring_lanelet_id = lanelet.neighboring_lanelet_id; - - // Copy linestrings data - for (int i = 0; i < 2; ++i) { - // Copy points from the original linestring to the new one if points are not empty - if (!lanelet.linestrings[i].points.empty()) { - segment.linestrings[i].poses.reserve(lanelet.linestrings[i].points.size()); - - for (const db_msgs::msg::DBPoint & point : lanelet.linestrings[i].points) { - segment.linestrings[i].poses.push_back(point.pose); - } - } else { - RCLCPP_WARN( - static_logger, - "Linestring does not contain points (ConvertLaneletsStamped2RoadSegments)!"); - } - } - - // Add the filled segment to the road_segments message - road_segments.segments.push_back(segment); - } - } - - return road_segments; -} - std::vector> GetAllSuccessorSequences( const std::vector & lanelet_connections, const int id_initial_lanelet) { diff --git a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp index 52cd721c62923..3b01ec8ac36a4 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp @@ -33,6 +33,15 @@ class LocalRoadProviderNode : public rclcpp::Node */ void CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg); + /** + * @brief Convert the LaneletsStamped message into a RoadSegments message. + * + * @param msg The message (db_msgs::msg::LaneletsStamped). + * @return autoware_planning_msgs::msg::RoadSegments. + */ + autoware_planning_msgs::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( + const db_msgs::msg::LaneletsStamped & msg); + // Declare ROS2 publisher and subscriber rclcpp::Publisher::SharedPtr road_publisher_; diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp index 098ff6f03c447..d2f57de668481 100644 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp @@ -28,6 +28,55 @@ LocalRoadProviderNode::LocalRoadProviderNode() : Node("local_road_provider_node" std::bind(&LocalRoadProviderNode::CallbackLaneletsMessages_, this, _1)); } +autoware_planning_msgs::msg::RoadSegments +LocalRoadProviderNode::ConvertLaneletsStamped2RoadSegments( + const db_msgs::msg::LaneletsStamped & msg) +{ + // Declare a static logger + static rclcpp::Logger static_logger = rclcpp::get_logger("static_logger"); + + // Initialize road segments message + autoware_planning_msgs::msg::RoadSegments road_segments; + + // Fill message header and pose + road_segments.header = msg.header; + road_segments.pose = msg.pose; + + // Convert lanelets to segments if lanelets are not empty + if (!msg.lanelets.empty()) { + for (const db_msgs::msg::Lanelet & lanelet : msg.lanelets) { + // Initialize a segment + autoware_planning_msgs::msg::Segment segment; + + // Fill the segment with basic information + segment.id = lanelet.id; + segment.successor_lanelet_id = lanelet.successor_lanelet_id; + segment.neighboring_lanelet_id = lanelet.neighboring_lanelet_id; + + // Copy linestrings data + for (int i = 0; i < 2; ++i) { + // Copy points from the original linestring to the new one if points are not empty + if (!lanelet.linestrings[i].points.empty()) { + segment.linestrings[i].poses.reserve(lanelet.linestrings[i].points.size()); + + for (const db_msgs::msg::DBPoint & point : lanelet.linestrings[i].points) { + segment.linestrings[i].poses.push_back(point.pose); + } + } else { + RCLCPP_WARN( + static_logger, + "Linestring does not contain points (ConvertLaneletsStamped2RoadSegments)!"); + } + } + + // Add the filled segment to the road_segments message + road_segments.segments.push_back(segment); + } + } + + return road_segments; +} + void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg) { autoware_planning_msgs::msg::RoadSegments road_segments = @@ -36,4 +85,5 @@ void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::Lanele // Publish the RoadSegments message road_publisher_->publish(road_segments); } + } // namespace autoware::mapless_architecture From c1958e0560b9cc4dd7d07200f8974b684863d6b7 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Wed, 26 Jun 2024 09:58:18 +0000 Subject: [PATCH 16/24] Remove node, change readme Signed-off-by: Simon Eisenmann --- planning/mapless_architecture/README.md | 13 --- .../CMakeLists.txt | 47 ---------- .../autoware_local_road_provider/Readme.md | 3 - .../local_road_provider_node.hpp | 53 ----------- .../launch/local_road_provider.launch.py | 27 ------ .../autoware_local_road_provider/package.xml | 25 ------ .../src/local_road_provider_main.cpp | 18 ---- .../src/local_road_provider_node.cpp | 89 ------------------- .../test/gtest_main.cpp | 9 -- 9 files changed, 284 deletions(-) delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/Readme.md delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/package.xml delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp delete mode 100644 planning/mapless_architecture/autoware_local_road_provider/test/gtest_main.cpp diff --git a/planning/mapless_architecture/README.md b/planning/mapless_architecture/README.md index caf496065ccdf..c987dc8eb309e 100644 --- a/planning/mapless_architecture/README.md +++ b/planning/mapless_architecture/README.md @@ -1,6 +1,3 @@ -[![pipeline status](https://gitlab.com/driveblocks/mod_mission_planner/badges/dev/pipeline.svg)](https://gitlab.com/driveblocks/mod_mission_planner/-/commits/dev) -[![coverage status](https://gitlab.com/driveblocks/mod_mission_planner/badges/dev/coverage.svg)](https://gitlab.com/driveblocks/mod_mission_planner/-/commits/dev) - # Mission Planner The Mission Planner module generates a reference trajectory/path in a local road model based on mission inputs. These inputs are received from the Human Machine Interface (HMI) or another agent. The resulting trajectory or path is then forwarded to the Behavior Planner for further processing. @@ -36,13 +33,3 @@ ros2 launch autoware_local_mission_planner mission_planner.launch.py ## Additional Notes During the beta phase, the mission planner will immediately output a straight trajectory with low velocity to move the vehicle into the local road model. Once the vehicle can be located in the local road model, a trajectory following the ego lane will be computed. - -## Styleguide - -This package adheres to the [Autoware styleguide](https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/languages/cpp/) which can be achieved via [`pre-commit`](https://autowarefoundation.github.io/autoware-documentation/pr-347/contributing/pull-request-guidelines/ci-checks/#pre-commit). Run - -```bash -pre-commit run -a -``` - -to format the code in the Autoware style and check the tests for errors which have to be solved manually. diff --git a/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt b/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt deleted file mode 100644 index a5bd0b689d0d6..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(autoware_local_road_provider) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# --- FIND DEPENDENCIES --- -find_package(autoware_cmake REQUIRED) # automatically find dependencies -ament_auto_find_build_dependencies() -autoware_package() - -# build executables -ament_auto_add_executable(${PROJECT_NAME} - src/local_road_provider_main.cpp - src/local_road_provider_node.cpp) - -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $) - -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - -# install executable(s) -install(TARGETS - ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME}) - -# install launchfile(s) [all within the "launch" folder] -install(DIRECTORY -launch -DESTINATION share/${PROJECT_NAME}) - -# --- SPECIFY TESTS --- -# configure clang format -set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - - ament_auto_add_gtest(${PROJECT_NAME}_tests test/gtest_main.cpp) - - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/planning/mapless_architecture/autoware_local_road_provider/Readme.md b/planning/mapless_architecture/autoware_local_road_provider/Readme.md deleted file mode 100644 index 147f05155c78f..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/Readme.md +++ /dev/null @@ -1,3 +0,0 @@ -# Local Road Provider Node - -This node converts the db_msgs::msg::LaneletsStamped message into a mission_planner_messages::msg::RoadSegments message right now. More functionality can be added later. diff --git a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp b/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp deleted file mode 100644 index 3b01ec8ac36a4..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/include/autoware/local_road_provider/local_road_provider_node.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license -#ifndef AUTOWARE__LOCAL_ROAD_PROVIDER__LOCAL_ROAD_PROVIDER_NODE_HPP_ -#define AUTOWARE__LOCAL_ROAD_PROVIDER__LOCAL_ROAD_PROVIDER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "autoware_planning_msgs/msg/road_segments.hpp" -#include "db_msgs/msg/lanelets_stamped.hpp" - -namespace autoware::mapless_architecture -{ - -/** - * Node for the Local Road Provider. - */ -class LocalRoadProviderNode : public rclcpp::Node -{ -public: - /** - * @brief Constructor for the LocalRoadProviderNode class. - * - * Initializes the publisher and subscriber with appropriate topics and QoS - * settings. - */ - LocalRoadProviderNode(); - -private: - /** - * @brief The callback for the LaneletsStamped messages. - * - * @param msg The db_msgs::msg::LaneletsStamped message. - */ - void CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg); - - /** - * @brief Convert the LaneletsStamped message into a RoadSegments message. - * - * @param msg The message (db_msgs::msg::LaneletsStamped). - * @return autoware_planning_msgs::msg::RoadSegments. - */ - autoware_planning_msgs::msg::RoadSegments ConvertLaneletsStamped2RoadSegments( - const db_msgs::msg::LaneletsStamped & msg); - - // Declare ROS2 publisher and subscriber - - rclcpp::Publisher::SharedPtr road_publisher_; - - rclcpp::Subscription::SharedPtr lanelets_subscriber_; -}; -} // namespace autoware::mapless_architecture - -#endif // AUTOWARE__LOCAL_ROAD_PROVIDER__LOCAL_ROAD_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py b/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py deleted file mode 100644 index ffb0293816857..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/launch/local_road_provider.launch.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2024 driveblocks GmbH -# driveblocks proprietary license -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription( - [ - # autoware_local_road_provider executable - Node( - package="autoware_local_road_provider", - executable="autoware_local_road_provider", - name="autoware_local_road_provider", - namespace="mapless_architecture", - remappings=[ - ( - "local_road_provider_node/output/road_segments", - "local_road_provider_node/output/road_segments", - ), - ("local_road_provider_node/input/lanelets", "/env/output/driving_corridors"), - ], - parameters=[], - output="screen", - ), - ] - ) diff --git a/planning/mapless_architecture/autoware_local_road_provider/package.xml b/planning/mapless_architecture/autoware_local_road_provider/package.xml deleted file mode 100644 index 190cd6deb71da..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - autoware_local_road_provider - 0.0.1 - local_road_provider - driveblocks - driveblocks proprietary license - - autoware_cmake - - ros2launch - - autoware_local_mission_planner_common - autoware_planning_msgs - db_msgs - rclcpp - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp deleted file mode 100644 index 7f47117d1a809..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_main.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license - -#include "autoware/local_road_provider/local_road_provider_node.hpp" - -#include - -#include - -int main(int argc, char * argv[]) -{ - RCLCPP_INFO(rclcpp::get_logger("local_road_provider"), "Launching Local Road Provider node..."); - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp b/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp deleted file mode 100644 index d2f57de668481..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/src/local_road_provider_node.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license - -#include "autoware/local_road_provider/local_road_provider_node.hpp" - -#include "autoware/local_mission_planner_common/helper_functions.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace autoware::mapless_architecture -{ -using std::placeholders::_1; - -LocalRoadProviderNode::LocalRoadProviderNode() : Node("local_road_provider_node") -{ - // Set quality of service to best effort (if transmission fails, do not try to - // resend but rather use new sensor data) - // the history_depth is set to 1 (message queue size) - auto qos = rclcpp::QoS(1); - qos.best_effort(); - - // Initialize publisher for road segments - road_publisher_ = this->create_publisher( - "local_road_provider_node/output/road_segments", 1); - - // Initialize subscriber to lanelets stamped messages - lanelets_subscriber_ = this->create_subscription( - "local_road_provider_node/input/lanelets", qos, - std::bind(&LocalRoadProviderNode::CallbackLaneletsMessages_, this, _1)); -} - -autoware_planning_msgs::msg::RoadSegments -LocalRoadProviderNode::ConvertLaneletsStamped2RoadSegments( - const db_msgs::msg::LaneletsStamped & msg) -{ - // Declare a static logger - static rclcpp::Logger static_logger = rclcpp::get_logger("static_logger"); - - // Initialize road segments message - autoware_planning_msgs::msg::RoadSegments road_segments; - - // Fill message header and pose - road_segments.header = msg.header; - road_segments.pose = msg.pose; - - // Convert lanelets to segments if lanelets are not empty - if (!msg.lanelets.empty()) { - for (const db_msgs::msg::Lanelet & lanelet : msg.lanelets) { - // Initialize a segment - autoware_planning_msgs::msg::Segment segment; - - // Fill the segment with basic information - segment.id = lanelet.id; - segment.successor_lanelet_id = lanelet.successor_lanelet_id; - segment.neighboring_lanelet_id = lanelet.neighboring_lanelet_id; - - // Copy linestrings data - for (int i = 0; i < 2; ++i) { - // Copy points from the original linestring to the new one if points are not empty - if (!lanelet.linestrings[i].points.empty()) { - segment.linestrings[i].poses.reserve(lanelet.linestrings[i].points.size()); - - for (const db_msgs::msg::DBPoint & point : lanelet.linestrings[i].points) { - segment.linestrings[i].poses.push_back(point.pose); - } - } else { - RCLCPP_WARN( - static_logger, - "Linestring does not contain points (ConvertLaneletsStamped2RoadSegments)!"); - } - } - - // Add the filled segment to the road_segments message - road_segments.segments.push_back(segment); - } - } - - return road_segments; -} - -void LocalRoadProviderNode::CallbackLaneletsMessages_(const db_msgs::msg::LaneletsStamped & msg) -{ - autoware_planning_msgs::msg::RoadSegments road_segments = - ConvertLaneletsStamped2RoadSegments(msg); - - // Publish the RoadSegments message - road_publisher_->publish(road_segments); -} - -} // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_road_provider/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_road_provider/test/gtest_main.cpp deleted file mode 100644 index d885ea084b1a3..0000000000000 --- a/planning/mapless_architecture/autoware_local_road_provider/test/gtest_main.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license -#include "gtest/gtest.h" - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 55b4e77628d818bc48d18ac0749684f92e283315 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Wed, 26 Jun 2024 14:47:56 +0000 Subject: [PATCH 17/24] Remove local_road_provider Signed-off-by: Simon Eisenmann --- .../launch/mission_planner_compose.launch.py | 17 +---------------- 1 file changed, 1 insertion(+), 16 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py index 0cab6abcafd47..fcf15de872045 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py @@ -44,19 +44,6 @@ def generate_launch_description(): # launch_arguments={'node_name': 'bar'}.items() ) - local_road_provider_pkg_share_directory = get_package_share_directory( - "autoware_local_road_provider" - ) - local_road_provider_file_name = "local_road_provider.launch.py" - local_road_provider_path = ( - Path(local_road_provider_pkg_share_directory) / "launch/" / local_road_provider_file_name - ) - - local_road_provider = IncludeLaunchDescription( - PythonLaunchDescriptionSource(str(local_road_provider_path)), - # launch_arguments={'node_name': 'bar'}.items() - ) - local_map_provider_pkg_share_directory = get_package_share_directory( "autoware_local_map_provider" ) @@ -70,6 +57,4 @@ def generate_launch_description(): # launch_arguments={'node_name': 'bar'}.items() ) - return LaunchDescription( - [mission_planner, mission_lane_converter, hmi, local_road_provider, local_map_provider] - ) + return LaunchDescription([mission_planner, mission_lane_converter, hmi, local_map_provider]) From 3bb548f5ffced3ecdfbe842964ec423eccc4b0eb Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 27 Jun 2024 07:10:42 +0000 Subject: [PATCH 18/24] Remove db_msgs Signed-off-by: Simon Eisenmann --- .../autoware_local_map_provider/package.xml | 1 - .../autoware/local_mission_planner/mission_planner_node.hpp | 4 ++-- .../autoware_local_mission_planner/package.xml | 1 - .../test/include/test_mission_planner_core.hpp | 2 -- .../autoware_local_mission_planner_common/CMakeLists.txt | 2 -- .../autoware_local_mission_planner_common/package.xml | 1 - 6 files changed, 2 insertions(+), 9 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_map_provider/package.xml b/planning/mapless_architecture/autoware_local_map_provider/package.xml index bee517a0f6752..6b581f565539b 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_map_provider/package.xml @@ -12,7 +12,6 @@ ros2launch autoware_planning_msgs - db_msgs lib_mission_planner rclcpp diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 403e6d834b811..c5242963c109d 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -207,7 +207,7 @@ class MissionPlannerNode : public rclcpp::Node /** * @brief Function for the visualization of lanes. * - * @param msg The db_msgs::msg::LaneletsStamped message. + * @param msg The autoware_planning_msgs::msg::RoadSegments message. * @param converted_lanelets The lanelets (std::vector). */ void VisualizeLanes_( @@ -217,7 +217,7 @@ class MissionPlannerNode : public rclcpp::Node /** * @brief Function for the visualization of the centerline of a driving corridor. * - * @param msg The db_msgs::msg::LaneletsStamped message. + * @param msg The autoware_planning_msgs::msg::RoadSegments message. * @param driving_corridor The considered driving corridor for which the centerline is visualized. */ void VisualizeCenterlineOfDrivingCorridor_( diff --git a/planning/mapless_architecture/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/autoware_local_mission_planner/package.xml index 3ba867af04bd6..85e1485316524 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner/package.xml @@ -14,7 +14,6 @@ autoware_local_mission_planner_common autoware_planning_msgs builtin_interfaces - db_msgs geometry_msgs lanelet2_core rclcpp diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp index f60e324a3eea2..d75852cee7c70 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp @@ -3,8 +3,6 @@ #ifndef TEST_MISSION_PLANNER_CORE_HPP_ #define TEST_MISSION_PLANNER_CORE_HPP_ -#include "db_msgs/msg/lanelets_stamped.hpp" - namespace autoware::mapless_architecture { diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt index 38096de1dc697..dc44c639668f1 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt @@ -22,7 +22,6 @@ ament_target_dependencies(${PROJECT_NAME} tf2 tf2_ros tf2_geometry_msgs - db_msgs lanelet2_core autoware_planning_msgs) @@ -40,7 +39,6 @@ ament_export_dependencies( tf2 tf2_ros tf2_geometry_msgs - db_msgs lanelet2_core autoware_planning_msgs) diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml index 780a0adf34fd4..f9923c4d33c3f 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml @@ -10,7 +10,6 @@ autoware_cmake autoware_planning_msgs - db_msgs geometry_msgs lanelet2_core tf2 From 9f9b92e2f4b00f80c6d0345f393faf40621f8639 Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Thu, 27 Jun 2024 09:46:36 +0000 Subject: [PATCH 19/24] Move some functions to the library Signed-off-by: Simon Eisenmann --- .../mission_planner_node.hpp | 138 ++--- .../src/mission_planner_node.cpp | 505 +++++------------- .../test/src/test_mission_planner_core.cpp | 24 +- .../CMakeLists.txt | 6 +- .../helper_functions.hpp | 85 +++ .../package.xml | 1 + .../src/helper_functions.cpp | 253 +++++++++ 7 files changed, 507 insertions(+), 505 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index c5242963c109d..45396b6650cfc 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -53,44 +53,6 @@ class MissionPlannerNode : public rclcpp::Node public: MissionPlannerNode(); - /** - * @brief Get a point on the given lane that is x meters away in x direction - * (using a projection). - * - * @param lane The given lane (std::vector) on which the point is - * created. - * @param x_distance The point is created x_distance meters (float) away from - * the vehicle (in x direction using a projection). - * @param converted_lanelets The lanelets (std::vector) from - * the road model. - * @return lanelet::BasicPoint2d - */ - lanelet::BasicPoint2d GetPointOnLane_( - const std::vector & lane, const float x_distance, - const std::vector & converted_lanelets); - - /** - * @brief Calculate the distance between a point and a LineString (Euclidean - * distance). - * - * @param linestring The LineString. - * @param point The point. - * @return double - */ - double CalculateDistanceBetweenPointAndLineString_( - const lanelet::ConstLineString2d & linestring, const lanelet::BasicPoint2d & point); - - /** - * @brief Recenter a point in a lanelet to its closest point on the centerline - * - * @param goal_point The input point which should be re-centered - * @param road_model The road model which contains the point to be re-centered - * @return lanelet::BasicPoint2d The re-centered point (which lies on the - * centerline of its lanelet) - */ - lanelet::BasicPoint2d RecenterGoalPoint( - const lanelet::BasicPoint2d & goal_point, const std::vector & road_model); - /** * @brief Function which checks if the vehicle is on the goal lane. * This functions returns a bool depending on whether the vehicle is on the @@ -135,23 +97,6 @@ class MissionPlannerNode : public rclcpp::Node const std::vector & converted_lanelets, std::vector & lanelet_connections); - /** - * @brief Function for creating a marker array. - * This functions creates a visualization_msgs::msg::MarkerArray from the - given input. - * - * @param centerline The centerline which is a LineString. - * @param left The left boundary which is a LineString. - * @param right The right boundary which is a LineString. - * @param msg The LaneletsStamped message. - * @return MarkerArray (visualization_msgs::msg::MarkerArray). - */ - visualization_msgs::msg::MarkerArray CreateMarkerArray_( - const std::vector & centerline, - const std::vector & left, - const std::vector & right, - const autoware_planning_msgs::msg::RoadSegments & msg); - /** * @brief Getter for goal_point_ * @@ -166,17 +111,6 @@ class MissionPlannerNode : public rclcpp::Node */ void goal_point(const lanelet::BasicPoint2d & goal_point); - /** - * @brief Create a DrivingCorridor object. - * - * @param lane The lane which is a std::vector containing all the indices - * of the lane. - * @param converted_lanelets The lanelets (std::vector). - * @return autoware_planning_msgs::msg::DrivingCorridor - */ - autoware_planning_msgs::msg::DrivingCorridor CreateDrivingCorridor_( - const std::vector & lane, const std::vector & converted_lanelets); - /** * @brief The callback for the Mission messages. * @@ -203,14 +137,40 @@ class MissionPlannerNode : public rclcpp::Node std::vector & out_lanelets, std::vector & out_lanelet_connections); -private: + /** + * @brief Get a point on the given lane that is x meters away in x direction + * (using a projection). + * + * @param lane The given lane (std::vector) on which the point is + * created. + * @param x_distance The point is created x_distance meters (float) away from + * the vehicle (in x direction using a projection). + * @param converted_lanelets The lanelets (std::vector) from + * the road model. + * @return lanelet::BasicPoint2d + */ + lanelet::BasicPoint2d GetPointOnLane( + const std::vector & lane, const float x_distance, + const std::vector & converted_lanelets); + + /** + * @brief Calculate the distance between a point and a LineString (Euclidean + * distance). + * + * @param linestring The LineString. + * @param point The point. + * @return double + */ + double CalculateDistanceBetweenPointAndLineString( + const lanelet::ConstLineString2d & linestring, const lanelet::BasicPoint2d & point); + /** * @brief Function for the visualization of lanes. * * @param msg The autoware_planning_msgs::msg::RoadSegments message. * @param converted_lanelets The lanelets (std::vector). */ - void VisualizeLanes_( + void VisualizeLanes( const autoware_planning_msgs::msg::RoadSegments & msg, const std::vector & converted_lanelets); @@ -220,19 +180,11 @@ class MissionPlannerNode : public rclcpp::Node * @param msg The autoware_planning_msgs::msg::RoadSegments message. * @param driving_corridor The considered driving corridor for which the centerline is visualized. */ - void VisualizeCenterlineOfDrivingCorridor_( + void VisualizeCenterlineOfDrivingCorridor( const autoware_planning_msgs::msg::RoadSegments & msg, const autoware_planning_msgs::msg::DrivingCorridor & driving_corridor); - /** - * @brief Function for creating a lanelet::LineString2d. - * - * @param points The considered points - * (std::vector). - * @return lanelet::LineString2d - */ - lanelet::LineString2d CreateLineString_(const std::vector & points); - +private: /** * @brief Callback for the odometry messages. * @@ -249,36 +201,6 @@ class MissionPlannerNode : public rclcpp::Node */ void InitiateLaneChange_(const Direction direction, const std::vector & neighboring_lane); - /** - * @brief Get all the neighbor lanelets (neighbor lane) of a specific lane on one side. - * - * @param lane The considered lane. - * @param lanelet_connections The lanelet connections. - * @param vehicle_side The side of the vehicle that is considered (enum). - * @return std::vector - */ - std::vector GetAllNeighborsOfLane( - const std::vector & lane, const std::vector & lanelet_connections, - const int vehicle_side); - - /** - * @brief Add the predecessor lanelet to a lane. - * - * @param lane_idx The considered lane. The predecessor lanelet is added to - * the front of the lane. - * @param lanelet_connections The lanelet connections. - * - */ - void InsertPredecessorLanelet( - std::vector & lane_idx, const std::vector & lanelet_connections); - - /** - * @brief Calculate the predecessors. - * - * @param lanelet_connections The lanelet connections. - */ - void CalculatePredecessors(std::vector & lanelet_connections); - // Declare ROS2 publisher and subscriber rclcpp::Subscription::SharedPtr mapSubscriber_; diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 6183b866b1866..7ce57f6484e4e 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -107,7 +107,7 @@ void MissionPlannerNode::CallbackLocalMapMessages_( std::vector converted_lanelets; ConvertInput2LaneletFormat(msg.road_segments, converted_lanelets, lanelet_connections); - MissionPlannerNode::VisualizeLanes_(msg.road_segments, converted_lanelets); + VisualizeLanes(msg.road_segments, converted_lanelets); // Get the lanes Lanes result = MissionPlannerNode::CalculateLanes_(converted_lanelets, lanelet_connections); @@ -163,7 +163,7 @@ void MissionPlannerNode::CallbackLocalMapMessages_( // Check if successful lane change if ( IsOnGoalLane_(egoLaneletIndex, goal_point_, converted_lanelets, lanelet_connections) && - CalculateDistanceBetweenPointAndLineString_( + CalculateDistanceBetweenPointAndLineString( converted_lanelets[egoLaneletIndex].centerline2d(), pointEgo) <= distance_to_centerline_threshold_) { // Reset mission to lane keeping @@ -222,27 +222,25 @@ void MissionPlannerNode::CallbackLocalMapMessages_( // Change this value // Create driving corridors and add them to the MissionLanesStamped message - lanes.ego_lane = MissionPlannerNode::CreateDrivingCorridor_(ego_lane_, converted_lanelets); - MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_(msg.road_segments, lanes.ego_lane); + lanes.ego_lane = CreateDrivingCorridor(ego_lane_, converted_lanelets); + VisualizeCenterlineOfDrivingCorridor(msg.road_segments, lanes.ego_lane); // Initialize driving corridor autoware_planning_msgs::msg::DrivingCorridor driving_corridor; if (!left_lanes.empty()) { for (const std::vector & lane : left_lanes) { - driving_corridor = MissionPlannerNode::CreateDrivingCorridor_(lane, converted_lanelets); + driving_corridor = CreateDrivingCorridor(lane, converted_lanelets); lanes.drivable_lanes_left.push_back(driving_corridor); - MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( - msg.road_segments, driving_corridor); + VisualizeCenterlineOfDrivingCorridor(msg.road_segments, driving_corridor); } } if (!right_lanes.empty()) { for (const std::vector & lane : right_lanes) { - driving_corridor = MissionPlannerNode::CreateDrivingCorridor_(lane, converted_lanelets); + driving_corridor = CreateDrivingCorridor(lane, converted_lanelets); lanes.drivable_lanes_right.push_back(driving_corridor); - MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( - msg.road_segments, driving_corridor); + VisualizeCenterlineOfDrivingCorridor(msg.road_segments, driving_corridor); } } @@ -429,30 +427,6 @@ void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry return; } -lanelet::BasicPoint2d MissionPlannerNode::RecenterGoalPoint( - const lanelet::BasicPoint2d & goal_point, const std::vector & road_model) -{ - // Return value - lanelet::BasicPoint2d projected_goal_point; - - // Get current lanelet index of goal point - const int lanelet_idx_goal_point = FindOccupiedLaneletID(road_model, goal_point); - - if (lanelet_idx_goal_point >= 0) { - // Get the centerline of the goal point's lanelet - lanelet::ConstLineString2d centerline_curr_lanelet = - road_model[lanelet_idx_goal_point].centerline2d(); - - // Project goal point to the centerline of its lanelet - projected_goal_point = lanelet::geometry::project(centerline_curr_lanelet, goal_point); - } else { - // Return untouched input point if index is not valid - projected_goal_point = goal_point; - } - - return projected_goal_point; -} - void MissionPlannerNode::CallbackMissionMessages_(const autoware_planning_msgs::msg::Mission & msg) { // Initialize variables @@ -509,78 +483,8 @@ void MissionPlannerNode::InitiateLaneChange_( mission_ = direction; target_lane_ = direction; goal_point_ = - GetPointOnLane_(neighboring_lane, projection_distance_on_goallane_, current_lanelets_); - } -} - -void MissionPlannerNode::VisualizeLanes_( - const autoware_planning_msgs::msg::RoadSegments & msg, - const std::vector & converted_lanelets) -{ - // Calculate centerlines, left and right bounds - std::vector centerlines; - std::vector left_bounds; - std::vector right_bounds; - - // Go through every lanelet - for (const lanelet::Lanelet & l : converted_lanelets) { - auto centerline = l.centerline(); - auto bound_left = l.leftBound(); - auto bound_right = l.rightBound(); - - centerlines.push_back(centerline); - left_bounds.push_back(bound_left); - right_bounds.push_back(bound_right); + GetPointOnLane(neighboring_lane, projection_distance_on_goallane_, current_lanelets_); } - - auto marker_array = - MissionPlannerNode::CreateMarkerArray_(centerlines, left_bounds, right_bounds, msg); - - // Publish centerlines, left and right bounds - visualizationPublisher_->publish(marker_array); -} - -void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor_( - const autoware_planning_msgs::msg::RoadSegments & msg, - const autoware_planning_msgs::msg::DrivingCorridor & driving_corridor) -{ - // Create a marker for the centerline - visualization_msgs::msg::Marker centerline_marker; - centerline_marker.header.frame_id = msg.header.frame_id; - centerline_marker.header.stamp = msg.header.stamp; - centerline_marker.ns = "centerline"; - - // Unique ID - if (centerline_marker_id_ == std::numeric_limits::max()) { - // Handle overflow - centerline_marker_id_ = 0; - } else { - centerline_marker.id = centerline_marker_id_++; - } - - centerline_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - centerline_marker.action = visualization_msgs::msg::Marker::ADD; - - // Set the scale of the marker - centerline_marker.scale.x = 0.1; // Line width - - // Set the color of the marker (red, green, blue, alpha) - centerline_marker.color.r = 1.0; - centerline_marker.color.g = 0.0; - centerline_marker.color.b = 0.0; - centerline_marker.color.a = 1.0; - - // Add points to the marker - for (const geometry_msgs::msg::Point & p : driving_corridor.centerline) { - centerline_marker.points.push_back(p); - } - - // Create a MarkerArray to hold the marker - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.push_back(centerline_marker); - - // Publish the marker array - visualization_publisher_centerline_->publish(marker_array); } // Determine the lanes @@ -668,223 +572,6 @@ Lanes MissionPlannerNode::CalculateLanes_( return lanes; } -void MissionPlannerNode::InsertPredecessorLanelet( - std::vector & lane_idx, const std::vector & lanelet_connections) -{ - if (!lane_idx.empty()) { - // Get index of first lanelet - int first_lanelet_index = lane_idx[0]; - - if (!lanelet_connections[first_lanelet_index].predecessor_lanelet_ids.empty()) { - // Get one predecessor lanelet - const int predecessor_lanelet = lanelet_connections[first_lanelet_index] - .predecessor_lanelet_ids[0]; // Get one of the predecessors - - // Insert predecessor lanelet in lane_idx - if (predecessor_lanelet >= 0) { - lane_idx.insert(lane_idx.begin(), predecessor_lanelet); - } - } - } -} - -std::vector MissionPlannerNode::GetAllNeighborsOfLane( - const std::vector & lane, const std::vector & lanelet_connections, - const int vehicle_side) -{ - // Initialize vector - std::vector neighbor_lane_idx = {}; - - if (!lane.empty()) { - // Loop through all the lane indices to get the neighbors - int neighbor_tmp; - - for (const int id : lane) { - neighbor_tmp = lanelet_connections[id].neighbor_lanelet_ids[vehicle_side]; - if (neighbor_tmp >= 0) { - // Only add neighbor if lanelet does not exist already (avoid having - // duplicates) - if ( - std::find(neighbor_lane_idx.begin(), neighbor_lane_idx.end(), neighbor_tmp) == - neighbor_lane_idx.end()) { - neighbor_lane_idx.push_back(neighbor_tmp); - } - } else { - // If there is a blind spot in the neighbor sequence, break the loop - break; - } - } - } - - return neighbor_lane_idx; -} - -visualization_msgs::msg::MarkerArray MissionPlannerNode::CreateMarkerArray_( - const std::vector & centerline, - const std::vector & left, - const std::vector & right, - const autoware_planning_msgs::msg::RoadSegments & msg) -{ - visualization_msgs::msg::MarkerArray markerArray; - - // Centerline - for (size_t i = 0; i < centerline.size(); ++i) { - visualization_msgs::msg::Marker marker; // Create a new marker in each iteration - - // Adding points to the marker - for (const auto & point : centerline[i]) { - geometry_msgs::msg::Point p; - - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - - marker.points.push_back(p); - } - markerArray.markers.push_back(marker); - } - - // Left bound - for (size_t i = 0; i < left.size(); ++i) { - visualization_msgs::msg::Marker marker; // Create a new marker in each iteration - - marker.header.frame_id = msg.header.frame_id; // Adjust frame_id as needed - marker.header.stamp = msg.header.stamp; // rclcpp::Node::now(); - marker.ns = "linestring"; - marker.id = i + 1000; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; // Neutral orientation - marker.scale.x = 5.0; - marker.color.b = 1.0; // Blue color - marker.color.a = 1.0; // Full opacity - - // Adding points to the marker - for (const auto & point : left[i]) { - geometry_msgs::msg::Point p; - - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - - marker.points.push_back(p); - } - markerArray.markers.push_back(marker); - } - - // Right bound - for (size_t i = 0; i < right.size(); ++i) { - visualization_msgs::msg::Marker marker; // Create a new marker in each iteration - - marker.header.frame_id = msg.header.frame_id; // Adjust frame_id as needed - marker.header.stamp = msg.header.stamp; // rclcpp::Node::now(); - marker.ns = "linestring"; - marker.id = i + 2000; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; // Neutral orientation - marker.scale.x = 5.0; - marker.color.b = 1.0; // Blue color - marker.color.a = 1.0; // Full opacity - - // Adding points to the marker - for (const auto & point : right[i]) { - geometry_msgs::msg::Point p; - - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - - marker.points.push_back(p); - } - markerArray.markers.push_back(marker); - } - - return markerArray; -} - -autoware_planning_msgs::msg::DrivingCorridor MissionPlannerNode::CreateDrivingCorridor_( - const std::vector & lane, const std::vector & converted_lanelets) -{ - // Create driving corridor - autoware_planning_msgs::msg::DrivingCorridor driving_corridor; - - for (int id : lane) { - if (id >= 0) { - auto current_lanelet = converted_lanelets.at(id); - - auto centerline = current_lanelet.centerline(); - auto bound_left = current_lanelet.leftBound(); - auto bound_right = current_lanelet.rightBound(); - - // Adding elements of centerline - for (const auto & point : centerline) { - geometry_msgs::msg::Point p; - - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - - driving_corridor.centerline.push_back(p); - } - - // Adding elements of bound_left - for (const auto & point : bound_left) { - geometry_msgs::msg::Point p; - - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - - driving_corridor.bound_left.push_back(p); - } - - // Adding elements of bound_right - for (const auto & point : bound_right) { - geometry_msgs::msg::Point p; - - p.x = point.x(); - p.y = point.y(); - p.z = point.z(); - - driving_corridor.bound_right.push_back(p); - } - } - } - return driving_corridor; -} - -lanelet::BasicPoint2d MissionPlannerNode::GetPointOnLane_( - const std::vector & lane, const float x_distance, - const std::vector & converted_lanelets) -{ - lanelet::BasicPoint2d return_point; // return value - - if (lane.size() > 0) { - lanelet::ConstLineString2d linestring = MissionPlannerNode::CreateLineString_( - MissionPlannerNode::CreateDrivingCorridor_(lane, converted_lanelets) - .centerline); // Create linestring for the lane - - // Create point that is float meters in front (x axis) - lanelet::BasicPoint2d point(x_distance, 0.0); - - // Get projected point on the linestring - lanelet::BasicPoint2d projected_point = lanelet::geometry::project(linestring, point); - - // Overwrite p (return value) - return_point.x() = projected_point.x(); - return_point.y() = projected_point.y(); - } else { - RCLCPP_WARN( - this->get_logger(), - "Overwriting of point may not have occurred properly. Lane is " - "probably empty."); - } - - // Return point p - return return_point; -} - bool MissionPlannerNode::IsOnGoalLane_( const int ego_lanelet_index, const lanelet::BasicPoint2d & goal_point, const std::vector & converted_lanelets, @@ -916,43 +603,6 @@ bool MissionPlannerNode::IsOnGoalLane_( return result; } -// Create LineString2d that consists of the given points (x and y) -lanelet::LineString2d MissionPlannerNode::CreateLineString_( - const std::vector & points) -{ - // Create a Lanelet2 linestring - lanelet::LineString2d linestring; - - // Iterate through the vector of points and add them to the linestring - for (const auto & point : points) { - lanelet::Point2d p(0, point.x, - point.y); // First argument is ID (set it to 0 for now) - linestring.push_back(p); - } - - // Return the created linestring - return linestring; -} - -// Function to calculate the shortest distance between a point and a -// linestring -double MissionPlannerNode::CalculateDistanceBetweenPointAndLineString_( - const lanelet::ConstLineString2d & linestring, const lanelet::BasicPoint2d & point) -{ - // Get projected point on the linestring - lanelet::BasicPoint2d projected_point = lanelet::geometry::project(linestring, point); - - // Calculate the distance between the two points - double distance = lanelet::geometry::distance2d(point, projected_point); - - // Publish distance - autoware_planning_msgs::msg::VisualizationDistance d; - d.distance = distance; - visualizationDistancePublisher_->publish(d); - - return distance; -} - void MissionPlannerNode::CheckIfGoalPointShouldBeReset_( const lanelet::Lanelets & converted_lanelets, const std::vector & lanelet_connections) @@ -967,7 +617,7 @@ void MissionPlannerNode::CheckIfGoalPointShouldBeReset_( if (goal_index >= 0) { // Check if -1 // Reset goal point - goal_point_ = GetPointOnLane_( + goal_point_ = GetPointOnLane( GetAllSuccessorSequences(lanelet_connections, goal_index)[0], projection_distance_on_goallane_, converted_lanelets); } else { @@ -1084,35 +734,126 @@ void MissionPlannerNode::ConvertInput2LaneletFormat( } // Fill predecessor field for each lanelet - this->CalculatePredecessors(out_lanelet_connections); + CalculatePredecessors(out_lanelet_connections); return; } -void MissionPlannerNode::CalculatePredecessors(std::vector & lanelet_connections) +lanelet::BasicPoint2d MissionPlannerNode::GetPointOnLane( + const std::vector & lane, const float x_distance, + const std::vector & converted_lanelets) { - // Determine predecessor information from already known information - for (std::size_t id_lanelet = 0; id_lanelet < lanelet_connections.size(); id_lanelet++) { - // Write lanelet predecessors (which are the successors of their previous - // lanelets) - for (std::size_t n_successor = 0; - n_successor < lanelet_connections[id_lanelet].successor_lanelet_ids.size(); - n_successor++) { - // Check if current lanelet has a valid successor, otherwise end of - // current local environment model has been reached and all the - // predecessors have been written with the last iteration - if (lanelet_connections[id_lanelet].successor_lanelet_ids[n_successor] > -1) { - lanelet_connections[lanelet_connections[id_lanelet].successor_lanelet_ids[n_successor]] - .predecessor_lanelet_ids.push_back(id_lanelet); - } - } + lanelet::BasicPoint2d return_point; // return value + + if (lane.size() > 0) { + lanelet::ConstLineString2d linestring = + CreateLineString(CreateDrivingCorridor(lane, converted_lanelets) + .centerline); // Create linestring for the lane + + // Create point that is float meters in front (x axis) + lanelet::BasicPoint2d point(x_distance, 0.0); + + // Get projected point on the linestring + lanelet::BasicPoint2d projected_point = lanelet::geometry::project(linestring, point); + + // Overwrite p (return value) + return_point.x() = projected_point.x(); + return_point.y() = projected_point.y(); + } else { + RCLCPP_WARN( + this->get_logger(), + "Overwriting of point may not have occurred properly. Lane is " + "probably empty."); } - // Write -1 to lanelets which have no predecessors - for (LaneletConnection lanelet_connection : lanelet_connections) { - if (lanelet_connection.predecessor_lanelet_ids.empty()) { - lanelet_connection.predecessor_lanelet_ids = {-1}; - } + // Return point p + return return_point; +} + +double MissionPlannerNode::CalculateDistanceBetweenPointAndLineString( + const lanelet::ConstLineString2d & linestring, const lanelet::BasicPoint2d & point) +{ + // Get projected point on the linestring + lanelet::BasicPoint2d projected_point = lanelet::geometry::project(linestring, point); + + // Calculate the distance between the two points + double distance = lanelet::geometry::distance2d(point, projected_point); + + // Publish distance + autoware_planning_msgs::msg::VisualizationDistance d; + d.distance = distance; + visualizationDistancePublisher_->publish(d); + + return distance; +} + +void MissionPlannerNode::VisualizeLanes( + const autoware_planning_msgs::msg::RoadSegments & msg, + const std::vector & converted_lanelets) +{ + // Calculate centerlines, left and right bounds + std::vector centerlines; + std::vector left_bounds; + std::vector right_bounds; + + // Go through every lanelet + for (const lanelet::Lanelet & l : converted_lanelets) { + auto centerline = l.centerline(); + auto bound_left = l.leftBound(); + auto bound_right = l.rightBound(); + + centerlines.push_back(centerline); + left_bounds.push_back(bound_left); + right_bounds.push_back(bound_right); + } + + auto marker_array = CreateMarkerArray(centerlines, left_bounds, right_bounds, msg); + + // Publish centerlines, left and right bounds + visualizationPublisher_->publish(marker_array); +} + +void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor( + const autoware_planning_msgs::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::DrivingCorridor & driving_corridor) +{ + // Create a marker for the centerline + visualization_msgs::msg::Marker centerline_marker; + centerline_marker.header.frame_id = msg.header.frame_id; + centerline_marker.header.stamp = msg.header.stamp; + centerline_marker.ns = "centerline"; + + // Unique ID + if (centerline_marker_id_ == std::numeric_limits::max()) { + // Handle overflow + centerline_marker_id_ = 0; + } else { + centerline_marker.id = centerline_marker_id_++; } + + centerline_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + centerline_marker.action = visualization_msgs::msg::Marker::ADD; + + // Set the scale of the marker + centerline_marker.scale.x = 0.1; // Line width + + // Set the color of the marker (red, green, blue, alpha) + centerline_marker.color.r = 1.0; + centerline_marker.color.g = 0.0; + centerline_marker.color.b = 0.0; + centerline_marker.color.a = 1.0; + + // Add points to the marker + for (const geometry_msgs::msg::Point & p : driving_corridor.centerline) { + centerline_marker.points.push_back(p); + } + + // Create a MarkerArray to hold the marker + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.push_back(centerline_marker); + + // Publish the marker array + visualization_publisher_centerline_->publish(marker_array); } + } // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index fe74b58bd88df..079b0993ef1a2 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -120,7 +120,7 @@ int TestCalculateDistanceBetweenPointAndLineString() MissionPlannerNode MissionPlanner = MissionPlannerNode(); // Run function - double distance = MissionPlanner.CalculateDistanceBetweenPointAndLineString_(linestring, point); + double distance = MissionPlanner.CalculateDistanceBetweenPointAndLineString(linestring, point); // Check if distance is near to 1.0 (with allowed error of 0.001) EXPECT_NEAR(distance, 1.0, 0.001); @@ -144,7 +144,7 @@ int TestGetPointOnLane() // Get a point from the tested function which has the x value 3.0 and lies on // the centerline of the lanelets - lanelet::BasicPoint2d point = MissionPlanner.GetPointOnLane_({0, 1}, 3.0, lanelets); + lanelet::BasicPoint2d point = MissionPlanner.GetPointOnLane({0, 1}, 3.0, lanelets); // Check if x value of the point is near to 3.0 (with an allowed error of // 0.001) @@ -152,8 +152,8 @@ int TestGetPointOnLane() // Get a point from the tested function which has the x value 100.0 and lies // on the centerline of the lanelets - point = MissionPlanner.GetPointOnLane_({0, 1}, 100.0, - lanelets); // Far away (100m) + point = MissionPlanner.GetPointOnLane({0, 1}, 100.0, + lanelets); // Far away (100m) // Check if x value of the point is near to 10.0 (with an allowed error of // 0.001) @@ -282,8 +282,7 @@ int TestRecenterGoalpoint() // TEST 1: point on centerline of origin lanelet // define a test point and re-center onto its centerline lanelet::BasicPoint2d goal_point(0.0, 0.0); - lanelet::BasicPoint2d goal_point_recentered = - mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + lanelet::BasicPoint2d goal_point_recentered = RecenterGoalPoint(goal_point, converted_lanelets); EXPECT_NEAR(goal_point_recentered.x(), 0.0, 1e-5); EXPECT_NEAR(goal_point_recentered.y(), 0.0, 1e-5); @@ -292,7 +291,7 @@ int TestRecenterGoalpoint() // define a test point which is not on the centerline goal_point.x() = 1.0; goal_point.y() = 0.25; - goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + goal_point_recentered = RecenterGoalPoint(goal_point, converted_lanelets); // Expect re-centered point to lie on y coordinate 0 EXPECT_NEAR(goal_point_recentered.x(), goal_point.x(), 1e-5); @@ -302,7 +301,7 @@ int TestRecenterGoalpoint() // define a test point which is not on the centerline goal_point.x() = 8.0; goal_point.y() = -0.2; - goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + goal_point_recentered = RecenterGoalPoint(goal_point, converted_lanelets); // Expect re-centered point to lie on y coordinate 0 but with x coord equal to // goal_point (removes lateral error/noise) @@ -314,7 +313,7 @@ int TestRecenterGoalpoint() // the centerline goal_point.x() = 2.0; goal_point.y() = 1.75; - goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + goal_point_recentered = RecenterGoalPoint(goal_point, converted_lanelets); // Expect re-centered point to lie on y coordinate 0 but with x coord equal to // goal_point (removes lateral error/noise) @@ -325,7 +324,7 @@ int TestRecenterGoalpoint() // untouched goal_point.x() = 11.0; goal_point.y() = -2.0; - goal_point_recentered = mission_planner.RecenterGoalPoint(goal_point, converted_lanelets); + goal_point_recentered = RecenterGoalPoint(goal_point, converted_lanelets); EXPECT_EQ(goal_point_recentered.x(), goal_point.x()); EXPECT_EQ(goal_point_recentered.y(), goal_point.y()); @@ -532,8 +531,7 @@ int TestCreateMarkerArray() } // Call function which is tested - auto marker_array = - MissionPlanner.CreateMarkerArray_(centerlines, left_bounds, right_bounds, message); + auto marker_array = CreateMarkerArray(centerlines, left_bounds, right_bounds, message); // Check if marker_array is empty EXPECT_EQ(marker_array.markers.empty(), @@ -562,7 +560,7 @@ int TestCreateDrivingCorridor() // Call function which is tested autoware_planning_msgs::msg::DrivingCorridor driving_corridor = - MissionPlanner.CreateDrivingCorridor_({0, 1}, lanelets); + CreateDrivingCorridor({0, 1}, lanelets); // Check if x value of first point in centerline is -2.0 EXPECT_EQ(driving_corridor.centerline[0].x, -2.0); diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt index dc44c639668f1..71ad1c1bc56a3 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt @@ -23,7 +23,8 @@ ament_target_dependencies(${PROJECT_NAME} tf2_ros tf2_geometry_msgs lanelet2_core - autoware_planning_msgs) + autoware_planning_msgs + visualization_msgs) # include public headers target_include_directories(${PROJECT_NAME} PUBLIC @@ -40,7 +41,8 @@ ament_export_dependencies( tf2_ros tf2_geometry_msgs lanelet2_core - autoware_planning_msgs) + autoware_planning_msgs + visualization_msgs) # install library install( diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index 7fb72b92ae249..0d5fd8cdda31f 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -9,9 +9,16 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "autoware_planning_msgs/msg/driving_corridor.hpp" +#include "autoware_planning_msgs/msg/local_map.hpp" +#include "autoware_planning_msgs/msg/mission.hpp" +#include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" #include "autoware_planning_msgs/msg/road_segments.hpp" +#include "autoware_planning_msgs/msg/visualization_distance.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include #include @@ -261,6 +268,84 @@ int FindOccupiedLaneletID( */ int FindEgoOccupiedLaneletID(const std::vector & lanelets); +/** + * @brief Recenter a point in a lanelet to its closest point on the centerline + * + * @param goal_point The input point which should be re-centered + * @param road_model The road model which contains the point to be re-centered + * @return lanelet::BasicPoint2d The re-centered point (which lies on the + * centerline of its lanelet) + */ +lanelet::BasicPoint2d RecenterGoalPoint( + const lanelet::BasicPoint2d & goal_point, const std::vector & road_model); + +/** + * @brief Function for creating a marker array. + * This functions creates a visualization_msgs::msg::MarkerArray from the + given input. + * + * @param centerline The centerline which is a LineString. + * @param left The left boundary which is a LineString. + * @param right The right boundary which is a LineString. + * @param msg The LaneletsStamped message. + * @return MarkerArray (visualization_msgs::msg::MarkerArray). + */ +visualization_msgs::msg::MarkerArray CreateMarkerArray( + const std::vector & centerline, + const std::vector & left, + const std::vector & right, + const autoware_planning_msgs::msg::RoadSegments & msg); + +/** + * @brief Create a DrivingCorridor object. + * + * @param lane The lane which is a std::vector containing all the indices + * of the lane. + * @param converted_lanelets The lanelets (std::vector). + * @return autoware_planning_msgs::msg::DrivingCorridor + */ +autoware_planning_msgs::msg::DrivingCorridor CreateDrivingCorridor( + const std::vector & lane, const std::vector & converted_lanelets); + +/** + * @brief Function for creating a lanelet::LineString2d. + * + * @param points The considered points + * (std::vector). + * @return lanelet::LineString2d + */ +lanelet::LineString2d CreateLineString(const std::vector & points); + +/** + * @brief Get all the neighbor lanelets (neighbor lane) of a specific lane on one side. + * + * @param lane The considered lane. + * @param lanelet_connections The lanelet connections. + * @param vehicle_side The side of the vehicle that is considered (enum). + * @return std::vector + */ +std::vector GetAllNeighborsOfLane( + const std::vector & lane, const std::vector & lanelet_connections, + const int vehicle_side); + +/** + * @brief Add the predecessor lanelet to a lane. + * + * @param lane_idx The considered lane. The predecessor lanelet is added to + * the front of the lane. + * @param lanelet_connections The lanelet connections. + * + */ +void InsertPredecessorLanelet( + std::vector & lane, const std::vector & lanelet_connections); + +/** + * @brief Calculate the predecessors. + * + * @param lanelet_connections The lanelet connections. + */ +void CalculatePredecessors(std::vector & lanelet_connections); + } // namespace autoware::mapless_architecture #endif // AUTOWARE__LOCAL_MISSION_PLANNER_COMMON__HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml index f9923c4d33c3f..f7537e8ad6250 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml @@ -15,6 +15,7 @@ tf2 tf2_geometry_msgs tf2_ros + visualization_msgs ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index 37e77b4ecaff1..7e0d1582cdffc 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -399,4 +399,257 @@ int FindEgoOccupiedLaneletID(const std::vector & lanelets) return FindOccupiedLaneletID(lanelets, position_ego); } +lanelet::BasicPoint2d RecenterGoalPoint( + const lanelet::BasicPoint2d & goal_point, const std::vector & road_model) +{ + // Return value + lanelet::BasicPoint2d projected_goal_point; + + // Get current lanelet index of goal point + const int lanelet_idx_goal_point = FindOccupiedLaneletID(road_model, goal_point); + + if (lanelet_idx_goal_point >= 0) { + // Get the centerline of the goal point's lanelet + lanelet::ConstLineString2d centerline_curr_lanelet = + road_model[lanelet_idx_goal_point].centerline2d(); + + // Project goal point to the centerline of its lanelet + projected_goal_point = lanelet::geometry::project(centerline_curr_lanelet, goal_point); + } else { + // Return untouched input point if index is not valid + projected_goal_point = goal_point; + } + + return projected_goal_point; +} + +visualization_msgs::msg::MarkerArray CreateMarkerArray( + const std::vector & centerline, + const std::vector & left, + const std::vector & right, + const autoware_planning_msgs::msg::RoadSegments & msg) +{ + visualization_msgs::msg::MarkerArray markerArray; + + // Centerline + for (size_t i = 0; i < centerline.size(); ++i) { + visualization_msgs::msg::Marker marker; // Create a new marker in each iteration + + // Adding points to the marker + for (const auto & point : centerline[i]) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + marker.points.push_back(p); + } + markerArray.markers.push_back(marker); + } + + // Left bound + for (size_t i = 0; i < left.size(); ++i) { + visualization_msgs::msg::Marker marker; // Create a new marker in each iteration + + marker.header.frame_id = msg.header.frame_id; // Adjust frame_id as needed + marker.header.stamp = msg.header.stamp; // rclcpp::Node::now(); + marker.ns = "linestring"; + marker.id = i + 1000; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; // Neutral orientation + marker.scale.x = 5.0; + marker.color.b = 1.0; // Blue color + marker.color.a = 1.0; // Full opacity + + // Adding points to the marker + for (const auto & point : left[i]) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + marker.points.push_back(p); + } + markerArray.markers.push_back(marker); + } + + // Right bound + for (size_t i = 0; i < right.size(); ++i) { + visualization_msgs::msg::Marker marker; // Create a new marker in each iteration + + marker.header.frame_id = msg.header.frame_id; // Adjust frame_id as needed + marker.header.stamp = msg.header.stamp; // rclcpp::Node::now(); + marker.ns = "linestring"; + marker.id = i + 2000; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; // Neutral orientation + marker.scale.x = 5.0; + marker.color.b = 1.0; // Blue color + marker.color.a = 1.0; // Full opacity + + // Adding points to the marker + for (const auto & point : right[i]) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + marker.points.push_back(p); + } + markerArray.markers.push_back(marker); + } + + return markerArray; +} + +autoware_planning_msgs::msg::DrivingCorridor CreateDrivingCorridor( + const std::vector & lane, const std::vector & converted_lanelets) +{ + // Create driving corridor + autoware_planning_msgs::msg::DrivingCorridor driving_corridor; + + for (int id : lane) { + if (id >= 0) { + auto current_lanelet = converted_lanelets.at(id); + + auto centerline = current_lanelet.centerline(); + auto bound_left = current_lanelet.leftBound(); + auto bound_right = current_lanelet.rightBound(); + + // Adding elements of centerline + for (const auto & point : centerline) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + driving_corridor.centerline.push_back(p); + } + + // Adding elements of bound_left + for (const auto & point : bound_left) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + driving_corridor.bound_left.push_back(p); + } + + // Adding elements of bound_right + for (const auto & point : bound_right) { + geometry_msgs::msg::Point p; + + p.x = point.x(); + p.y = point.y(); + p.z = point.z(); + + driving_corridor.bound_right.push_back(p); + } + } + } + return driving_corridor; +} + +lanelet::LineString2d CreateLineString(const std::vector & points) +{ + // Create a Lanelet2 linestring + lanelet::LineString2d linestring; + + // Iterate through the vector of points and add them to the linestring + for (const auto & point : points) { + lanelet::Point2d p(0, point.x, + point.y); // First argument is ID (set it to 0 for now) + linestring.push_back(p); + } + + // Return the created linestring + return linestring; +} + +std::vector GetAllNeighborsOfLane( + const std::vector & lane, const std::vector & lanelet_connections, + const int vehicle_side) +{ + // Initialize vector + std::vector neighbor_lane_idx = {}; + + if (!lane.empty()) { + // Loop through all the lane indices to get the neighbors + int neighbor_tmp; + + for (const int id : lane) { + neighbor_tmp = lanelet_connections[id].neighbor_lanelet_ids[vehicle_side]; + if (neighbor_tmp >= 0) { + // Only add neighbor if lanelet does not exist already (avoid having + // duplicates) + if ( + std::find(neighbor_lane_idx.begin(), neighbor_lane_idx.end(), neighbor_tmp) == + neighbor_lane_idx.end()) { + neighbor_lane_idx.push_back(neighbor_tmp); + } + } else { + // If there is a blind spot in the neighbor sequence, break the loop + break; + } + } + } + + return neighbor_lane_idx; +} + +void InsertPredecessorLanelet( + std::vector & lane_idx, const std::vector & lanelet_connections) +{ + if (!lane_idx.empty()) { + // Get index of first lanelet + int first_lanelet_index = lane_idx[0]; + + if (!lanelet_connections[first_lanelet_index].predecessor_lanelet_ids.empty()) { + // Get one predecessor lanelet + const int predecessor_lanelet = lanelet_connections[first_lanelet_index] + .predecessor_lanelet_ids[0]; // Get one of the predecessors + + // Insert predecessor lanelet in lane_idx + if (predecessor_lanelet >= 0) { + lane_idx.insert(lane_idx.begin(), predecessor_lanelet); + } + } + } +} + +void CalculatePredecessors(std::vector & lanelet_connections) +{ + // Determine predecessor information from already known information + for (std::size_t id_lanelet = 0; id_lanelet < lanelet_connections.size(); id_lanelet++) { + // Write lanelet predecessors (which are the successors of their previous + // lanelets) + for (std::size_t n_successor = 0; + n_successor < lanelet_connections[id_lanelet].successor_lanelet_ids.size(); + n_successor++) { + // Check if current lanelet has a valid successor, otherwise end of + // current local environment model has been reached and all the + // predecessors have been written with the last iteration + if (lanelet_connections[id_lanelet].successor_lanelet_ids[n_successor] > -1) { + lanelet_connections[lanelet_connections[id_lanelet].successor_lanelet_ids[n_successor]] + .predecessor_lanelet_ids.push_back(id_lanelet); + } + } + } + + // Write -1 to lanelets which have no predecessors + for (LaneletConnection lanelet_connection : lanelet_connections) { + if (lanelet_connection.predecessor_lanelet_ids.empty()) { + lanelet_connection.predecessor_lanelet_ids = {-1}; + } + } +} + } // namespace autoware::mapless_architecture From 6219caa0e2f96284c443d7fd806c5c4dec05f14d Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Wed, 3 Jul 2024 12:17:23 +0000 Subject: [PATCH 20/24] Merge commit Signed-off-by: Simon Eisenmann --- .../src/mission_planner_node.cpp | 18 +++++++------- .../test/src/test_mission_planner_core.cpp | 24 +++++++++---------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 7ce57f6484e4e..c7adb93e2ac2b 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -697,15 +697,15 @@ void MissionPlannerNode::ConvertInput2LaneletFormat( // Get successor/neighbor lanelet information // Write lanelet neighbors - for (std::size_t i = 0; i < msg.segments[idx_segment].neighboring_lanelet_id.size(); i++) { + for (std::size_t i = 0; i < msg.segments[idx_segment].neighboring_segment_id.size(); i++) { out_lanelet_connections[idx_segment].neighbor_lanelet_ids.push_back( - msg.segments[idx_segment].neighboring_lanelet_id[i]); + msg.segments[idx_segment].neighboring_segment_id[i]); } // Write lanelet successors - for (std::size_t i = 0; i < msg.segments[idx_segment].successor_lanelet_id.size(); i++) { + for (std::size_t i = 0; i < msg.segments[idx_segment].successor_segment_id.size(); i++) { out_lanelet_connections[idx_segment].successor_lanelet_ids.push_back( - msg.segments[idx_segment].successor_lanelet_id[i]); + msg.segments[idx_segment].successor_segment_id[i]); } // The goal_information is not needed in this context, we set it to true for now @@ -713,12 +713,12 @@ void MissionPlannerNode::ConvertInput2LaneletFormat( } // Define lambda function to replace old id with a new one - auto ReplaceAndWarnIfNotFound = [&](auto & lanelet_ids) { - for (auto & lanelet_id : lanelet_ids) { - if (lanelet_id >= 0) { - auto it = map_original_to_new.find(lanelet_id); + auto ReplaceAndWarnIfNotFound = [&](auto & segment_ids) { + for (auto & segment_id : segment_ids) { + if (segment_id >= 0) { + auto it = map_original_to_new.find(segment_id); if (it != map_original_to_new.end()) { - lanelet_id = it->second; + segment_id = it->second; } else { // Key %i seems to be not present in the provided lanelet. } diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index 079b0993ef1a2..584c4ffdbbb26 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -88,12 +88,12 @@ autoware_planning_msgs::msg::RoadSegments CreateSegments() message.segments[2].linestrings[1].poses[1].position.z = 0.0; // Define connections - message.segments[0].neighboring_lanelet_id = {0, 1}; - message.segments[1].neighboring_lanelet_id = {0, 1}; - message.segments[2].neighboring_lanelet_id = {-1, -1}; - message.segments[0].successor_lanelet_id = {2}; - message.segments[1].successor_lanelet_id = {-1}; - message.segments[2].successor_lanelet_id = {-1}; + message.segments[0].neighboring_segment_id = {0, 1}; + message.segments[1].neighboring_segment_id = {0, 1}; + message.segments[2].neighboring_segment_id = {-1, -1}; + message.segments[0].successor_segment_id = {2}; + message.segments[1].successor_segment_id = {-1}; + message.segments[2].successor_segment_id = {-1}; return message; } @@ -257,9 +257,9 @@ autoware_planning_msgs::msg::RoadSegments GetTestRoadModelForRecenterTests() int32_t neigh_1 = 1; std::vector neighboring_ids = {neigh_1}; - message.segments[0].neighboring_lanelet_id = neighboring_ids; + message.segments[0].neighboring_segment_id = neighboring_ids; std::vector neighboring_ids_2 = {}; - message.segments[1].neighboring_lanelet_id = neighboring_ids_2; + message.segments[1].neighboring_segment_id = neighboring_ids_2; return message; } @@ -449,10 +449,10 @@ std::tuple, std::vector> Create message.segments[1].linestrings[1].poses[1].position.z = 0.0; // Define connections - message.segments[0].successor_lanelet_id = {1}; - message.segments[0].neighboring_lanelet_id = {-1, -1}; - message.segments[1].successor_lanelet_id = {-1}; - message.segments[1].neighboring_lanelet_id = {-1, -1}; + message.segments[0].successor_segment_id = {1}; + message.segments[0].neighboring_segment_id = {-1, -1}; + message.segments[1].successor_segment_id = {-1}; + message.segments[1].neighboring_segment_id = {-1, -1}; // Initialize MissionPlannerNode MissionPlannerNode MissionPlanner = MissionPlannerNode(); From 1d562b386faf22f6e8c4b3ccbc30219695118b2d Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Tue, 9 Jul 2024 07:43:55 +0000 Subject: [PATCH 21/24] 5: Local odom adaptions --- .../mission_planner_node.hpp | 3 +- .../param/mission_planner_default.yaml | 1 + .../src/mission_planner_node.cpp | 114 +++--------------- .../mission_lane_converter_node.hpp | 5 +- .../param/mission_lane_converter_default.yaml | 1 + .../src/mission_lane_converter_node.cpp | 47 ++++---- 6 files changed, 50 insertions(+), 121 deletions(-) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 45396b6650cfc..20b447f9a52ef 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -231,7 +231,7 @@ class MissionPlannerNode : public rclcpp::Node // Initialize some variables Pose2D pose_prev_; bool pose_prev_init_ = false; - bool b_global_odometry_deprecation_warning_ = false; + bool b_input_odom_frame_error_ = false; bool received_motion_update_once_ = false; Direction target_lane_ = stay; Direction mission_ = stay; @@ -249,6 +249,7 @@ class MissionPlannerNode : public rclcpp::Node float distance_to_centerline_threshold_; float projection_distance_on_goallane_; int retrigger_attempts_max_; + std::string local_map_frame_; // Unique ID for each marker int centerline_marker_id_ = 0; diff --git a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml index 27ee3f4e89d24..0de58c803a1a2 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml +++ b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml @@ -6,3 +6,4 @@ # if ego is in proximity to the goal centerline projection_distance_on_goallane: 20.0 # [m] projection distance of goal point of a mission retrigger_attempts_max: 10 # Number of attempts for triggering a lane change + local_map_frame: map # Identifier of local map frame. Currently, there is no way to set global ROS params https://github.com/ros2/ros2cli/issues/778 -> This param has to be set in the mission converter also! diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index c7adb93e2ac2b..6403235401dca 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -97,6 +97,9 @@ MissionPlannerNode::MissionPlannerNode() : Node("mission_planner_node") RCLCPP_INFO( this->get_logger(), "Number of attempts for triggering a lane change: %d", retrigger_attempts_max_); + + local_map_frame_ = declare_parameter("local_map_frame", "map"); + RCLCPP_INFO(this->get_logger(), "Local map frame identifier: %s", local_map_frame_.c_str()); } void MissionPlannerNode::CallbackLocalMapMessages_( @@ -250,111 +253,37 @@ void MissionPlannerNode::CallbackLocalMapMessages_( void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg) { - // NOTE: We assume that the odometry message is the GNSS signal - // Construct raw odometry pose - geometry_msgs::msg::PoseStamped odometry_pose_raw, pose_base_link_in_odom_frame, - pose_base_link_in_map_frame; + geometry_msgs::msg::PoseStamped odometry_pose_raw; odometry_pose_raw.header = msg.header; odometry_pose_raw.pose = msg.pose.pose; // If the incoming odometry signal is properly filled, i.e. if the frame ids - // are given and report an odometry signal , do nothing, else we assume the + // are given and report an odometry signal, do nothing, else we assume the // odometry signal stems from the GNSS (and is therefore valid in the odom // frame) - if (msg.header.frame_id == "map" && msg.child_frame_id == "base_link") { - pose_base_link_in_map_frame = odometry_pose_raw; - } else { - if (!b_global_odometry_deprecation_warning_) { - RCLCPP_WARN( + if (!(msg.header.frame_id == local_map_frame_ && msg.child_frame_id == "base_link")) { + if (!b_input_odom_frame_error_) { + RCLCPP_ERROR( this->get_logger(), "Your odometry signal doesn't match the expectation to be a " - "transformation from frame to ! We assume the " - "input " - "signal to be a GNSS raw signal being a transform from to " - ". The support for this feature will be deprecated in a " - "future " - "release, please check you odometry signal or use a driveblocks " - "local odometry signal instead! This warning is printed only " - "once."); - b_global_odometry_deprecation_warning_ = true; + "transformation from frame <%s> to ! The node will continue spinning but the " + "odometry signal should be checked! This error is printed only " + "once.", + local_map_frame_.c_str()); + b_input_odom_frame_error_ = true; } - // Prepare map to odom transform - // TODO(thomas.herrmann@driveblocks.ai): Can be removed when the state - // estimator publishes this information in the correct frames - geometry_msgs::msg::TransformStamped trafo_map2odom; - trafo_map2odom.header.stamp = msg.header.stamp; - trafo_map2odom.header.frame_id = "map"; - trafo_map2odom.child_frame_id = "odom"; - trafo_map2odom.transform.translation.x = msg.pose.pose.position.x; - trafo_map2odom.transform.translation.y = msg.pose.pose.position.y; - trafo_map2odom.transform.translation.z = msg.pose.pose.position.z; - trafo_map2odom.transform.rotation.x = msg.pose.pose.orientation.x; - trafo_map2odom.transform.rotation.y = msg.pose.pose.orientation.y; - trafo_map2odom.transform.rotation.z = msg.pose.pose.orientation.z; - trafo_map2odom.transform.rotation.w = msg.pose.pose.orientation.w; - - geometry_msgs::msg::TransformStamped trafo_base_link_in_odom_frame; - try { - // constant trafo from gnss receiver to base_link - trafo_base_link_in_odom_frame = - tf_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "Transform not yet available from to "); - } - - // Extract the trafo from the odom frame to the base_link frame - pose_base_link_in_odom_frame.header.frame_id = "odom"; - pose_base_link_in_odom_frame.pose.position.x = - trafo_base_link_in_odom_frame.transform.translation.x; - pose_base_link_in_odom_frame.pose.position.y = - trafo_base_link_in_odom_frame.transform.translation.y; - pose_base_link_in_odom_frame.pose.position.z = - trafo_base_link_in_odom_frame.transform.translation.z; - pose_base_link_in_odom_frame.pose.orientation.x = - trafo_base_link_in_odom_frame.transform.rotation.x; - pose_base_link_in_odom_frame.pose.orientation.y = - trafo_base_link_in_odom_frame.transform.rotation.y; - pose_base_link_in_odom_frame.pose.orientation.z = - trafo_base_link_in_odom_frame.transform.rotation.z; - pose_base_link_in_odom_frame.pose.orientation.w = - trafo_base_link_in_odom_frame.transform.rotation.w; - - // Transform base_link origin from odom frame to map frame - tf2::doTransform(pose_base_link_in_odom_frame, pose_base_link_in_map_frame, trafo_map2odom); } // Calculate yaw for received pose - double psi_cur_corrected = GetYawFromQuaternion( - pose_base_link_in_map_frame.pose.orientation.x, pose_base_link_in_map_frame.pose.orientation.y, - pose_base_link_in_map_frame.pose.orientation.z, pose_base_link_in_map_frame.pose.orientation.w); - - RCLCPP_DEBUG( - this->get_logger(), - "GNSS pose raw: x,y,z | quaternion: %.3f, %.3f, %.3f | %.3f, %.3f, " - "%.3f, " - "%.3f\t " - "GNSS pose in base_link: x,y,z | quaternion: %.3f, %.3f, %.3f | %.3f, " - "%.3f, " - "%.3f, %.3f", - odometry_pose_raw.pose.position.x, odometry_pose_raw.pose.position.y, - odometry_pose_raw.pose.position.z, odometry_pose_raw.pose.orientation.x, - odometry_pose_raw.pose.orientation.y, odometry_pose_raw.pose.orientation.z, - odometry_pose_raw.pose.orientation.w, pose_base_link_in_map_frame.pose.position.x, - pose_base_link_in_map_frame.pose.position.y, pose_base_link_in_map_frame.pose.position.z, - pose_base_link_in_map_frame.pose.orientation.x, pose_base_link_in_map_frame.pose.orientation.y, - pose_base_link_in_map_frame.pose.orientation.z, pose_base_link_in_map_frame.pose.orientation.w); - - RCLCPP_DEBUG( - this->get_logger(), "Received pose (x: %.2f, y: %.2f, psi %.2f)", - pose_base_link_in_map_frame.pose.position.x, pose_base_link_in_map_frame.pose.position.y, - psi_cur_corrected); + double psi_cur = GetYawFromQuaternion( + odometry_pose_raw.pose.orientation.x, odometry_pose_raw.pose.orientation.y, + odometry_pose_raw.pose.orientation.z, odometry_pose_raw.pose.orientation.w); if (pose_prev_init_) { // Calculate and forward relative motion update to driving corridor model const Pose2D pose_cur( - pose_base_link_in_map_frame.pose.position.x, pose_base_link_in_map_frame.pose.position.y, - psi_cur_corrected); + odometry_pose_raw.pose.position.x, odometry_pose_raw.pose.position.y, psi_cur); const Pose2D d_pose = TransformToNewCosy2D(pose_prev_, pose_cur); // Transform the target pose into the new cosy which is given in relation @@ -369,9 +298,6 @@ void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry // Re-center updated goal point to lie on centerline (to get rid of issues // with a less accurate odometry update which could lead to loosing the // goal lane) - // TODO(thomas.herrmann@driveblocks.ai): Reduction of calling - // frequency of this method (since not needed at a high frequency and - // probably computationally expensive) const lanelet::BasicPoint2d target_point_2d = RecenterGoalPoint(goal_point_, current_lanelets_); // Overwrite goal point @@ -418,9 +344,9 @@ void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry } // Update pose storage for next iteration - pose_prev_.set_x(pose_base_link_in_map_frame.pose.position.x); - pose_prev_.set_y(pose_base_link_in_map_frame.pose.position.y); - pose_prev_.set_psi(psi_cur_corrected); + pose_prev_.set_x(odometry_pose_raw.pose.position.x); + pose_prev_.set_y(odometry_pose_raw.pose.position.y); + pose_prev_.set_psi(psi_cur); received_motion_update_once_ = true; diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp index 2807b6c1ab834..75c4e35bbb636 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -178,8 +178,8 @@ class MissionLaneConverterNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; - // Switch to print a warning about wrongly configured odometry frames - bool b_global_odometry_deprecation_warning_ = false; + // Switch to print an error about wrongly configured odometry frames + bool b_input_odom_frame_error_ = false; bool received_motion_update_once_ = false; // Store initial and last available odom messages @@ -190,6 +190,7 @@ class MissionLaneConverterNode : public rclcpp::Node // ROS parameters float target_speed_; + std::string local_map_frame_; }; } // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml b/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml index e1255342f18dc..294bc38e830c9 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml +++ b/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml @@ -2,3 +2,4 @@ mission_lane_converter: ros__parameters: target_speed: 3.2 # [mps] constant target speed of the mission trajectories + local_map_frame: map diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 381d28dd9fb7b..79571026fc270 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -87,8 +87,8 @@ void MissionLaneConverterNode::TimedStartupTrajectoryCallback() autoware_auto_planning_msgs::msg::Trajectory trj_msg = autoware_auto_planning_msgs::msg::Trajectory(); - // Frame id must be "map" for Autoware controller - trj_msg.header.frame_id = "map"; + // Frame id + trj_msg.header.frame_id = local_map_frame_; trj_msg.header.stamp = rclcpp::Node::now(); for (int idx_point = 0; idx_point < 100; idx_point++) { @@ -219,9 +219,8 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( // Fill output trajectory header trj_msg.header = msg.header; path_msg.header = msg.header; - // Frame id must be "map" for Autoware controller - trj_msg.header.frame_id = "map"; - path_msg.header.frame_id = "map"; + trj_msg.header.frame_id = local_map_frame_; + path_msg.header.frame_id = local_map_frame_; switch (msg.target_lane) { case 0: @@ -492,32 +491,32 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) { // Define output message T msg_output = msg_input; - msg_output.header.frame_id = "map"; + msg_output.header.frame_id = local_map_frame_; // Construct raw odometry pose - geometry_msgs::msg::PoseStamped odometry_pose_raw, pose_base_link_in_odom_frame, - pose_base_link_in_map_frame; + geometry_msgs::msg::PoseStamped odometry_pose_raw; odometry_pose_raw.header = last_odom_msg_.header; odometry_pose_raw.pose = last_odom_msg_.pose.pose; - // If the incoming odometry signal is properly filled, i.e. if the frame ids - // are given and report an odometry signal , do nothing, else we assume the - // odometry signal stems from the GNSS (and is therefore valid in the odom - // frame) - if (last_odom_msg_.header.frame_id == "map" && last_odom_msg_.child_frame_id == "base_link") { - pose_base_link_in_map_frame = odometry_pose_raw; - } else { - if (!b_global_odometry_deprecation_warning_) { - RCLCPP_WARN( - this->get_logger(), - "Your odometry signal doesn't match the expectation to be a " - "transformation from frame to ! Check your odometry frames or provide a " - "proper conversion!"); - b_global_odometry_deprecation_warning_ = true; + if (received_motion_update_once_) { + // If the incoming odometry signal is properly filled, i.e. if the frame ids + // are given and report an odometry signal, do nothing, else we assume the + // odometry signal stems from the GNSS (and is therefore valid in the odom + // frame) + if (!(last_odom_msg_.header.frame_id == local_map_frame_ && + last_odom_msg_.child_frame_id == "base_link")) { + if (!b_input_odom_frame_error_) { + RCLCPP_ERROR( + this->get_logger(), + "Your odometry signal doesn't match the expectation to be a " + "transformation from frame <%s> to ! The node will continue spinning but the " + "odometry signal should be checked! This error is printed only " + "once.", + local_map_frame_.c_str()); + b_input_odom_frame_error_ = true; + } } - } - if (received_motion_update_once_) { const double psi_initial = GetYawFromQuaternion( initial_odom_msg_.pose.pose.orientation.x, initial_odom_msg_.pose.pose.orientation.y, initial_odom_msg_.pose.pose.orientation.z, initial_odom_msg_.pose.pose.orientation.w); From 9e137dfee454cc56ba1cc4261f1fb2ec44a05f9f Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Tue, 9 Jul 2024 09:13:03 +0000 Subject: [PATCH 22/24] 2: Work on requested changes --- planning/.pages | 7 +- .../autoware_hmi/CMakeLists.txt | 34 +++++--- .../autoware_hmi/Readme.md | 12 +++ .../include/autoware/hmi/hmi_node.hpp | 2 +- .../autoware_hmi/launch/hmi.launch.py | 2 +- .../autoware_hmi/package.xml | 1 + .../autoware_hmi/src/hmi_main.cpp | 18 ----- .../autoware_hmi/src/hmi_node.cpp | 6 +- .../CMakeLists.txt | 35 +++++--- .../autoware_local_map_provider/Readme.md | 12 +++ .../local_map_provider_node.hpp | 2 +- .../launch/local_map_provider.launch.py | 2 +- .../autoware_local_map_provider/package.xml | 1 + .../src/local_map_provider_main.cpp | 18 ----- .../src/local_map_provider_node.cpp | 7 +- .../CMakeLists.txt | 34 +++++--- .../autoware_local_mission_planner/Readme.md | 22 +++++ .../mission_planner_node.hpp | 2 +- .../launch/mission_planner.launch.py | 3 +- .../package.xml | 1 + .../param/mission_planner_default.yaml | 8 +- .../src/mission_planner_main.cpp | 16 ---- .../src/mission_planner_node.cpp | 7 +- .../test/src/test_mission_planner_core.cpp | 28 ++++--- .../CMakeLists.txt | 24 +++--- .../CMakeLists.txt | 35 +++++--- .../autoware_mission_lane_converter/Readme.md | 22 ++++- .../mission_lane_converter_node.hpp | 36 ++++----- .../launch/mission_lane_converter.launch.py | 2 +- .../package.xml | 2 +- .../src/main_mission_lane_converter.cpp | 18 ----- .../src/mission_lane_converter_node.cpp | 81 ++++++++++--------- .../src/test_mission_planner_converter.cpp | 10 ++- 33 files changed, 286 insertions(+), 224 deletions(-) delete mode 100644 planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp delete mode 100644 planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp delete mode 100644 planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp delete mode 100644 planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp diff --git a/planning/.pages b/planning/.pages index 0b9bb27181c1b..f4abad35aef25 100644 --- a/planning/.pages +++ b/planning/.pages @@ -39,7 +39,12 @@ nav: - 'About Freespace Planner': planning/autoware_freespace_planner - 'Algorithm': planning/autoware_freespace_planning_algorithms - 'RRT*': planning/autoware_freespace_planning_algorithms/rrtstar - - 'Mission Planner': planning/autoware_mission_planner + - 'Mapless Architecture': planning/mapless_architecture + - 'Human Machine Interface': planning/mapless_architecture/autoware_hmi + - 'Local Map Provider': planning/mapless_architecture/autoware_local_map_provider + - 'Local Mission Planner': planning/mapless_architecture/autoware_local_mission_planner + - 'Local Mission Planner Library': planning/mapless_architecture/autoware_local_mission_planner_common + - 'Mission Lane Converter': planning/mapless_architecture/autoware_mission_lane_converter - 'Local Mission Planner': planning/mapless_architecture - 'Motion Planning': - 'Path Optimizer': diff --git a/planning/mapless_architecture/autoware_hmi/CMakeLists.txt b/planning/mapless_architecture/autoware_hmi/CMakeLists.txt index 1bc934c5b3360..9de77f224e8b6 100644 --- a/planning/mapless_architecture/autoware_hmi/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_hmi/CMakeLists.txt @@ -1,40 +1,46 @@ cmake_minimum_required(VERSION 3.8) project(autoware_hmi) +# Check for compiler if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # --- FIND DEPENDENCIES --- -find_package(autoware_cmake REQUIRED) # automatically find dependencies +find_package(autoware_cmake REQUIRED) +find_package(rclcpp_components REQUIRED) ament_auto_find_build_dependencies() autoware_package() -# build executables -ament_auto_add_executable(${PROJECT_NAME} - src/hmi_main.cpp - src/hmi_node.cpp) +ament_auto_add_library(${PROJECT_NAME} SHARED + src/hmi_node.cpp +) +# Register node +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::mapless_architecture::HMINode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +# Specify include directories target_include_directories(${PROJECT_NAME} PUBLIC $ $) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) -# install executable(s) +# Install the target library install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) -# install launchfile(s) [all within the "launch" folder] +# Install the launch directory install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) # --- SPECIFY TESTS --- -# configure clang format -set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -44,4 +50,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package() +# Ensure all packages are correctly installed +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/planning/mapless_architecture/autoware_hmi/Readme.md b/planning/mapless_architecture/autoware_hmi/Readme.md index d84dd5231b921..aa74343559f6f 100644 --- a/planning/mapless_architecture/autoware_hmi/Readme.md +++ b/planning/mapless_architecture/autoware_hmi/Readme.md @@ -15,3 +15,15 @@ Interact with this node by changing the ROS parameters. For a lane change to the ```bash ros2 param set /mission_planner/hmi mission LANE_CHANGE_RIGHT ``` + +## Output topics + +| Name | Type | Description | +| ------------------------- | ------------------------------------ | ----------- | +| `hmi_node/output/mission` | autoware_planning_msgs::msg::Mission | mission | + +## Node parameters + +| Parameter | Type | Description | +| --------- | ------ | ---------------------------------------------- | +| `mission` | string | the mission (LANE_KEEP, LANE_CHANGE_LEFT, ...) | diff --git a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp index efb9726683686..fc090a93cc3a4 100644 --- a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp +++ b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp @@ -24,7 +24,7 @@ class HMINode : public rclcpp::Node * Initializes the publisher and subscriber with appropriate topics and QoS * settings. */ - HMINode(); + explicit HMINode(const rclcpp::NodeOptions & options); private: /** diff --git a/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py index 86ddd999c250c..7544a2d9f098e 100644 --- a/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py +++ b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): # hmi executable Node( package="autoware_hmi", - executable="autoware_hmi", + executable="autoware_hmi_exe", name="autoware_hmi", namespace="mapless_architecture", remappings=[ diff --git a/planning/mapless_architecture/autoware_hmi/package.xml b/planning/mapless_architecture/autoware_hmi/package.xml index 888cfa42bab6f..274112c5a27d8 100644 --- a/planning/mapless_architecture/autoware_hmi/package.xml +++ b/planning/mapless_architecture/autoware_hmi/package.xml @@ -13,6 +13,7 @@ autoware_planning_msgs rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp deleted file mode 100644 index 9dbcada942bf1..0000000000000 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_main.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license - -#include "autoware/hmi/hmi_node.hpp" - -#include - -#include - -int main(int argc, char * argv[]) -{ - RCLCPP_INFO(rclcpp::get_logger("hmi"), "Launching HMI node..."); - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index bf0add6ece583..8e600dc95bfa6 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -11,7 +11,7 @@ namespace autoware::mapless_architecture { using std::placeholders::_1; -HMINode::HMINode() : Node("hmi_node") +HMINode::HMINode(const rclcpp::NodeOptions & options) : Node("hmi_node", options) { // Set quality of service to best effort (if transmission fails, do not try to // resend but rather use new sensor data) @@ -78,3 +78,7 @@ void HMINode::PublishMission_(std::string mission) mission_publisher_->publish(missionMessage); } } // namespace autoware::mapless_architecture + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mapless_architecture::HMINode) diff --git a/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt b/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt index 2321e54572bc1..9e3cbee014f49 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt @@ -1,40 +1,47 @@ cmake_minimum_required(VERSION 3.8) project(autoware_local_map_provider) +# Check for compiler if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # --- FIND DEPENDENCIES --- -find_package(autoware_cmake REQUIRED) # automatically find dependencies +find_package(autoware_cmake REQUIRED) +find_package(rclcpp_components REQUIRED) ament_auto_find_build_dependencies() autoware_package() -# build executables -ament_auto_add_executable(${PROJECT_NAME} - src/local_map_provider_main.cpp - src/local_map_provider_node.cpp) +# Add the library +ament_auto_add_library(${PROJECT_NAME} SHARED + src/local_map_provider_node.cpp +) +# Register node +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::mapless_architecture::LocalMapProviderNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +# Specify include directories target_include_directories(${PROJECT_NAME} PUBLIC $ $) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) -# install executable(s) +# Install the target library install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) -# install launchfile(s) [all within the "launch" folder] +# Install the launch directory install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) # --- SPECIFY TESTS --- -# configure clang format -set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -44,4 +51,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package() +# Ensure all packages are correctly installed +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/planning/mapless_architecture/autoware_local_map_provider/Readme.md b/planning/mapless_architecture/autoware_local_map_provider/Readme.md index 251178ec6cac8..d5d39c74ef958 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/Readme.md +++ b/planning/mapless_architecture/autoware_local_map_provider/Readme.md @@ -1,3 +1,15 @@ # Local Map Provider Node This node converts the mission_planner_messages::msg::RoadSegments message into a mission_planner_messages::msg::LocalMap message right now. More functionality can be added later. + +## Input topics + +| Name | Type | Description | +| --------------------------------------------- | ----------------------------------------- | ------------- | +| `local_map_provider_node/input/road_segments` | autoware_planning_msgs::msg::RoadSegments | road segments | + +## Output topics + +| Name | Type | Description | +| ------------------------------------------ | ------------------------------------- | ----------- | +| `local_map_provider_node/output/local_map` | autoware_planning_msgs::msg::LocalMap | local map | diff --git a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp index 5c34b61c161fd..5a8effd39f11d 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp @@ -23,7 +23,7 @@ class LocalMapProviderNode : public rclcpp::Node * Initializes the publisher and subscriber with appropriate topics and QoS * settings. */ - LocalMapProviderNode(); + explicit LocalMapProviderNode(const rclcpp::NodeOptions & options); private: /** diff --git a/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py index 95de7e19abb75..ccdbd6c1a9ea9 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py +++ b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): # autoware_local_map_provider executable Node( package="autoware_local_map_provider", - executable="autoware_local_map_provider", + executable="autoware_local_map_provider_exe", name="autoware_local_map_provider", namespace="mapless_architecture", remappings=[ diff --git a/planning/mapless_architecture/autoware_local_map_provider/package.xml b/planning/mapless_architecture/autoware_local_map_provider/package.xml index 6b581f565539b..c5fa7680f9f1f 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/package.xml +++ b/planning/mapless_architecture/autoware_local_map_provider/package.xml @@ -14,6 +14,7 @@ autoware_planning_msgs lib_mission_planner rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp deleted file mode 100644 index 4ab340c49d077..0000000000000 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_main.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license - -#include "autoware/local_map_provider/local_map_provider_node.hpp" - -#include - -#include - -int main(int argc, char * argv[]) -{ - RCLCPP_INFO(rclcpp::get_logger("local_map_provider"), "Launching Local Map Provider node..."); - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp index 27e15fed30e00..726a396406f77 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -9,7 +9,8 @@ namespace autoware::mapless_architecture { using std::placeholders::_1; -LocalMapProviderNode::LocalMapProviderNode() : Node("local_map_provider_node") +LocalMapProviderNode::LocalMapProviderNode(const rclcpp::NodeOptions & options) +: Node("local_map_provider_node", options) { // Set quality of service to best effort (if transmission fails, do not try to // resend but rather use new sensor data) @@ -40,3 +41,7 @@ void LocalMapProviderNode::CallbackRoadSegmentsMessages_( local_map); // Outlook: Add global map, sign detection etc. to the message } } // namespace autoware::mapless_architecture + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mapless_architecture::LocalMapProviderNode) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt index d4f25246d0f28..2e5f431086536 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt @@ -1,41 +1,47 @@ cmake_minimum_required(VERSION 3.8) project(autoware_local_mission_planner) +# Check for compiler if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # --- FIND DEPENDENCIES --- -find_package(autoware_cmake REQUIRED) # automatically find dependencies +find_package(autoware_cmake REQUIRED) +find_package(rclcpp_components REQUIRED) ament_auto_find_build_dependencies() autoware_package() -# build executables -ament_auto_add_executable(${PROJECT_NAME} - src/mission_planner_main.cpp - src/mission_planner_node.cpp) +ament_auto_add_library(${PROJECT_NAME} SHARED + src/mission_planner_node.cpp +) +# Register node +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::mapless_architecture::MissionPlannerNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +# Specify include directories target_include_directories(${PROJECT_NAME} PUBLIC $ $) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) -# install executable(s) +# Install the target library install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) -# install launchfile(s)/param file(s) +# Install the launch and param directories install(DIRECTORY launch param DESTINATION share/${PROJECT_NAME}) # --- SPECIFY TESTS --- -# configure clang format -set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -47,4 +53,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package() +# Ensure all packages are correctly installed +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/Readme.md b/planning/mapless_architecture/autoware_local_mission_planner/Readme.md index e9f2caf9af130..b7d98ae933556 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/Readme.md +++ b/planning/mapless_architecture/autoware_local_mission_planner/Readme.md @@ -7,3 +7,25 @@ Here, one can see the target lane: ![target lane](images/Targetlane.png) The two plots show the point in time, when the lane change was triggered. + +## Input topics + +| Name | Type | Description | +| -------------------------------------- | ------------------------------------- | ----------- | +| `mission_planner_node/input/local_map` | autoware_planning_msgs::msg::LocalMap | local map | +| `mission_planner/input/mission` | autoware_planning_msgs::msg::Mission | mission | +| `mission_planner/input/state_estimate` | nav_msgs::msg::Odometry | odometry | + +## Output topics + +| Name | Type | Description | +| --------------------------------------------------- | ------------------------------------------------ | ------------- | +| `mission_planner_node/output/mission_lanes_stamped` | autoware_planning_msgs::msg::MissionLanesStamped | mission lanes | + +## Node parameters + +| Parameter | Type | Description | +| ---------------------------------- | ----- | ------------------------------------------------------------------------------------------------------------ | +| `distance_to_centerline_threshold` | float | threshold to determine if lane change mission was successful (if ego is in proximity to the goal centerline) | +| `projection_distance_on_goallane` | float | projection distance of goal point | +| `retrigger_attempts_max` | int | number of attempts for triggering a lane change | diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 20b447f9a52ef..099d41e68c15b 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -51,7 +51,7 @@ struct Lanes class MissionPlannerNode : public rclcpp::Node { public: - MissionPlannerNode(); + explicit MissionPlannerNode(const rclcpp::NodeOptions & options); /** * @brief Function which checks if the vehicle is on the goal lane. diff --git a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py index 100da396941e0..a6f56913832f2 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): # mission_planner executable Node( package="autoware_local_mission_planner", - executable="autoware_local_mission_planner", + executable="autoware_local_mission_planner_exe", name="autoware_local_mission_planner", namespace="mapless_architecture", remappings=[ @@ -55,7 +55,6 @@ def generate_launch_description(): LaunchConfiguration("mission_planner_param_file"), ], output="screen", - # prefix="gdbserver localhost:5000", ), ] ) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/autoware_local_mission_planner/package.xml index 85e1485316524..1e81bf9998f7d 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/package.xml +++ b/planning/mapless_architecture/autoware_local_mission_planner/package.xml @@ -17,6 +17,7 @@ geometry_msgs lanelet2_core rclcpp + rclcpp_components tf2 tf2_geometry_msgs tf2_ros diff --git a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml index 0de58c803a1a2..a95d5d3db2dc0 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml +++ b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml @@ -1,9 +1,7 @@ /mission_planner: mission_planner: ros__parameters: - distance_to_centerline_threshold: - 0.25 # [m] threshold to determine if lane change mission was successful - # if ego is in proximity to the goal centerline - projection_distance_on_goallane: 20.0 # [m] projection distance of goal point of a mission - retrigger_attempts_max: 10 # Number of attempts for triggering a lane change + distance_to_centerline_threshold: 0.25 # [m] threshold to determine if lane change mission was successful (if ego is in proximity to the goal centerline) + projection_distance_on_goallane: 20.0 # [m] projection distance of goal point + retrigger_attempts_max: 10 # number of attempts for triggering a lane change local_map_frame: map # Identifier of local map frame. Currently, there is no way to set global ROS params https://github.com/ros2/ros2cli/issues/778 -> This param has to be set in the mission converter also! diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp deleted file mode 100644 index 9b1f612c85de9..0000000000000 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_main.cpp +++ /dev/null @@ -1,16 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license -#include "autoware/local_mission_planner/mission_planner_node.hpp" -#include "rclcpp/rclcpp.hpp" - -#include - -int main(int argc, char * argv[]) -{ - RCLCPP_INFO(rclcpp::get_logger("mission_planner"), "Launching mission planner node..."); - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 6403235401dca..3b17ff32361b2 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -30,7 +30,8 @@ namespace autoware::mapless_architecture { using std::placeholders::_1; -MissionPlannerNode::MissionPlannerNode() : Node("mission_planner_node") +MissionPlannerNode::MissionPlannerNode(const rclcpp::NodeOptions & options) +: Node("mission_planner_node", options) { // Set quality of service to best effort (if transmission fails, do not try to // resend but rather use new sensor data) @@ -783,3 +784,7 @@ void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor( } } // namespace autoware::mapless_architecture + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mapless_architecture::MissionPlannerNode) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index 584c4ffdbbb26..424c7e562132c 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -4,6 +4,7 @@ #include "autoware/local_mission_planner/mission_planner_node.hpp" #include "autoware/local_mission_planner_common/helper_functions.hpp" #include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -117,7 +118,8 @@ int TestCalculateDistanceBetweenPointAndLineString() lanelet::BasicPoint2d point(1.0, 1.0); // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Run function double distance = MissionPlanner.CalculateDistanceBetweenPointAndLineString(linestring, point); @@ -134,7 +136,8 @@ int TestGetPointOnLane() autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Convert road model std::vector lanelet_connections; @@ -168,7 +171,8 @@ int TestIsOnGoalLane() autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Convert road model std::vector lanelet_connections; @@ -267,7 +271,8 @@ autoware_planning_msgs::msg::RoadSegments GetTestRoadModelForRecenterTests() int TestRecenterGoalpoint() { // Create a mission planner - MissionPlannerNode mission_planner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode mission_planner = MissionPlannerNode(options); // Get a local road model for testing autoware_planning_msgs::msg::RoadSegments road_segments = GetTestRoadModelForRecenterTests(); @@ -341,7 +346,8 @@ int TestCheckIfGoalPointShouldBeReset() local_map.road_segments = road_segments; // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Convert road model std::vector lanelet_connections; @@ -455,7 +461,8 @@ std::tuple, std::vector> Create message.segments[1].neighboring_segment_id = {-1, -1}; // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Output std::vector lanelet_connections; @@ -474,7 +481,8 @@ int TestCalculateLanes() std::vector lanelet_connections = std::get<1>(tuple); // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Call function which is tested Lanes result = MissionPlanner.CalculateLanes_(lanelets, lanelet_connections); @@ -510,7 +518,8 @@ int TestCreateMarkerArray() std::vector lanelet_connections = std::get<1>(tuple); // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Create empty message autoware_planning_msgs::msg::RoadSegments message; @@ -556,7 +565,8 @@ int TestCreateDrivingCorridor() std::vector lanelet_connections = std::get<1>(tuple); // Initialize MissionPlannerNode - MissionPlannerNode MissionPlanner = MissionPlannerNode(); + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); // Call function which is tested autoware_planning_msgs::msg::DrivingCorridor driving_corridor = diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt index 71ad1c1bc56a3..ddef091ae0f89 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt @@ -5,18 +5,18 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies -find_package(autoware_cmake REQUIRED) # automatically find dependencies +# Find dependencies +find_package(autoware_cmake REQUIRED) ament_auto_find_build_dependencies() autoware_package() -# add library to be exported +# Add library to be exported add_library(${PROJECT_NAME} SHARED src/helper_functions.cpp) include_directories(include) -# add dependent libraries +# Add dependent libraries ament_target_dependencies(${PROJECT_NAME} geometry_msgs tf2 @@ -26,15 +26,15 @@ ament_target_dependencies(${PROJECT_NAME} autoware_planning_msgs visualization_msgs) -# include public headers +# Include public headers target_include_directories(${PROJECT_NAME} PUBLIC $ $) -# export library +# Export library ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -# export dependent libraries +# Export dependent libraries ament_export_dependencies( geometry_msgs tf2 @@ -44,7 +44,7 @@ ament_export_dependencies( autoware_planning_msgs visualization_msgs) -# install library +# Install library install( DIRECTORY include/ DESTINATION include @@ -59,22 +59,18 @@ install( INCLUDES DESTINATION include ) -# configure clang format ----------------------------------------------------------------------------------------------- -set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) - -# testing -------------------------------------------------------------------------------------------------------------- +# Testing if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_auto_add_gtest(${PROJECT_NAME}_tests - # MAIN test/gtest_main.cpp - # src/helper_functions.cpp) ament_lint_auto_find_test_dependencies() target_include_directories(${PROJECT_NAME}_tests PRIVATE test/include) endif() +# Ensure all packages are correctly installed ament_package() diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt b/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt index b8be6a1efd25b..0f76de9b43e89 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt +++ b/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt @@ -1,41 +1,48 @@ cmake_minimum_required(VERSION 3.8) project(autoware_mission_lane_converter) +# Check for compiler if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # --- FIND DEPENDENCIES --- -find_package(autoware_cmake REQUIRED) # automatically find dependencies +find_package(autoware_cmake REQUIRED) +find_package(rclcpp_components REQUIRED) ament_auto_find_build_dependencies() autoware_package() -# build executables -ament_auto_add_executable(${PROJECT_NAME} - src/main_mission_lane_converter.cpp - src/mission_lane_converter_node.cpp) +# Add the library +ament_auto_add_library(${PROJECT_NAME} SHARED + src/mission_lane_converter_node.cpp +) +# Register node +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::mapless_architecture::MissionLaneConverterNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + +# Specify include directories target_include_directories(${PROJECT_NAME} PUBLIC $ $) -target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) -# install executable(s) +# Install the target library install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) -# install launchfile(s)/param file(s) +# Install the launch and param directories install(DIRECTORY launch param DESTINATION share/${PROJECT_NAME}) # --- SPECIFY TESTS --- -# configure clang format -set(ament_cmake_clang_format_CONFIG_FILE ../../.clang-format) - if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -47,4 +54,8 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_auto_package() +# Ensure all packages are correctly installed +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md b/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md index c2a4ec1e6fb7d..26956e2d110a1 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md +++ b/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md @@ -2,6 +2,24 @@ Converts the selected mission lane to an autoware trajectory. -## Build +## Input topics -In order to build the code, clone the TIER IV version of the autoware ROS messages from and include in your build. +| Name | Type | Description | +| -------------------------------------------- | ------------------------------------------------ | ------------- | +| `mission_lane_converter/input/odometry` | nav_msgs::msg::Odometry | odometry | +| `mission_lane_converter/input/mission_lanes` | autoware_planning_msgs::msg::MissionLanesStamped | mission lanes | + +## Output topics + +| Name | Type | Description | +| ------------------------------------------------- | --------------------------------------- | ----------------- | +| `mission_lane_converter/output/trajectory` | autoware_planning_msgs::msg::Trajectory | trajectory | +| `mission_lane_converter/output/global_trajectory` | autoware_planning_msgs::msg::Trajectory | global trajectory | +| `mission_lane_converter/output/path` | autoware_planning_msgs::msg::Path | path | +| `mission_lane_converter/output/global_path` | autoware_planning_msgs::msg::Path | global path | + +## Node parameters + +| Parameter | Type | Description | +| -------------- | ----- | ------------ | +| `target_speed` | float | target speed | diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp index 75c4e35bbb636..209006541d0a6 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -5,9 +5,9 @@ #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -25,20 +25,20 @@ namespace autoware::mapless_architecture class MissionLaneConverterNode : public rclcpp::Node { public: - MissionLaneConverterNode(); + explicit MissionLaneConverterNode(const rclcpp::NodeOptions & options); /** * @brief Converts the mission message into a reference trajectory which is * forwarded to a local trajectory planner for refinement. * * @param msg The mission lanes - * @return std::tuple */ std::tuple< - autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, - autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + autoware_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> ConvertMissionToTrajectory(const autoware_planning_msgs::msg::MissionLanesStamped & msg); private: @@ -58,7 +58,7 @@ class MissionLaneConverterNode : public rclcpp::Node * @param v_x The longitudinal velocity of the point to be added */ void AddTrajectoryPoint_( - autoware_auto_planning_msgs::msg::Trajectory & trj_msg, const double x, const double y, + autoware_planning_msgs::msg::Trajectory & trj_msg, const double x, const double y, const double v_x); /** @@ -78,7 +78,7 @@ class MissionLaneConverterNode : public rclcpp::Node * * @param trj_msg */ - void AddHeadingToTrajectory_(autoware_auto_planning_msgs::msg::Trajectory & trj_msg); + void AddHeadingToTrajectory_(autoware_planning_msgs::msg::Trajectory & trj_msg); /** * @brief Timed callback which shall be executed until a first valid local @@ -111,8 +111,7 @@ class MissionLaneConverterNode : public rclcpp::Node *@param v_x The v_x value. */ void AddPathPoint_( - autoware_auto_planning_msgs::msg::Path & pth_msg, const double x, const double y, - const double v_x); + autoware_planning_msgs::msg::Path & pth_msg, const double x, const double y, const double v_x); /** *@brief Create a motion planner input. @@ -124,9 +123,8 @@ class MissionLaneConverterNode : public rclcpp::Node *@param centerline_mission_lane The centerline of the mission lane. */ void CreateMotionPlannerInput_( - autoware_auto_planning_msgs::msg::Trajectory & trj_msg, - autoware_auto_planning_msgs::msg::Path & path_msg, visualization_msgs::msg::Marker & trj_vis, - visualization_msgs::msg::Marker & path_vis, + autoware_planning_msgs::msg::Trajectory & trj_msg, autoware_planning_msgs::msg::Path & path_msg, + visualization_msgs::msg::Marker & trj_vis, visualization_msgs::msg::Marker & path_vis, const std::vector & centerline_mission_lane); /** @@ -140,7 +138,7 @@ class MissionLaneConverterNode : public rclcpp::Node * @brief Template to transform both Autoware::Path and Autoware::Trajectory into a global map * frame. * - * @tparam T autoware_auto_planning_msgs::msg::Path, autoware_auto_planning_msgs::msg::Trajectory + * @tparam T autoware_planning_msgs::msg::Path, autoware_planning_msgs::msg::Trajectory * @param input Input ROS message which content must be transformed into the global map frame * @return T Same as input message with the content being valid in the global map */ @@ -154,7 +152,7 @@ class MissionLaneConverterNode : public rclcpp::Node * @return visualization_msgs::msg::Marker */ visualization_msgs::msg::Marker GetGlobalTrjVisualization_( - const autoware_auto_planning_msgs::msg::Trajectory & trj_msg); + const autoware_planning_msgs::msg::Trajectory & trj_msg); // Declare ROS2 publisher and subscriber @@ -163,10 +161,10 @@ class MissionLaneConverterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr mission_lane_subscriber_; - rclcpp::Publisher::SharedPtr trajectory_publisher_, + rclcpp::Publisher::SharedPtr trajectory_publisher_, trajectory_publisher_global_; - rclcpp::Publisher::SharedPtr path_publisher_, + rclcpp::Publisher::SharedPtr path_publisher_, path_publisher_global_; rclcpp::Publisher::SharedPtr vis_trajectory_publisher_, @@ -174,7 +172,7 @@ class MissionLaneConverterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr vis_path_publisher_; - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py index 6b35a12545828..3fecc4845e730 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py +++ b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): # mission lane converter executable Node( package="autoware_mission_lane_converter", - executable="autoware_mission_lane_converter", + executable="autoware_mission_lane_converter_exe", name="autoware_mission_lane_converter", namespace="mapless_architecture", remappings=[ diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml index 08167412ca035..cff328dd6cc18 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/package.xml +++ b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml @@ -11,11 +11,11 @@ ros2launch - autoware_auto_planning_msgs autoware_local_mission_planner_common autoware_planning_msgs geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs visualization_msgs diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp deleted file mode 100644 index da4616e3df094..0000000000000 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/main_mission_lane_converter.cpp +++ /dev/null @@ -1,18 +0,0 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license -#include "autoware/mission_lane_converter/mission_lane_converter_node.hpp" -#include "rclcpp/rclcpp.hpp" - -#include - -int main(int argc, char * argv[]) -{ - RCLCPP_INFO( - rclcpp::get_logger("mission_planner_converter_node"), - "Launching mission lane converter node..."); - - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 79571026fc270..1da3d197950eb 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -6,10 +6,10 @@ #include "autoware/local_mission_planner_common/helper_functions.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "visualization_msgs/msg/marker.hpp" #include @@ -20,7 +20,8 @@ namespace autoware::mapless_architecture { using std::placeholders::_1; -MissionLaneConverterNode::MissionLaneConverterNode() : Node("mission_lane_converter_node") +MissionLaneConverterNode::MissionLaneConverterNode(const rclcpp::NodeOptions & options) +: Node("mission_lane_converter_node", options) { // Set quality of service to best effort (if transmission fails, do not try to // resend but rather use new sensor data) @@ -41,22 +42,22 @@ MissionLaneConverterNode::MissionLaneConverterNode() : Node("mission_lane_conver std::bind(&MissionLaneConverterNode::MissionLanesCallback_, this, _1)); // Initialize publisher - trajectory_publisher_ = this->create_publisher( + trajectory_publisher_ = this->create_publisher( "mission_lane_converter/output/trajectory", qos_reliability); - trajectory_publisher_global_ = - this->create_publisher( - "mission_lane_converter/output/global_trajectory", qos_reliability); + trajectory_publisher_global_ = this->create_publisher( + "mission_lane_converter/output/global_trajectory", qos_reliability); - // Artificial publisher to test the trajectory generation - publisher_ = this->create_publisher( + // Artificial publisher to test the trajectory generation, TODO(simon.eisenmann@driveblocks.ai): + // Remove this publisher later, it is needed to get the vehicle started + publisher_ = this->create_publisher( "mission_lane_converter/output/global_trajectory", qos_reliability); // Path publisher - path_publisher_ = create_publisher( + path_publisher_ = create_publisher( "mission_lane_converter/output/path", qos_reliability); - path_publisher_global_ = create_publisher( + path_publisher_global_ = create_publisher( "mission_lane_converter/output/global_path", qos_reliability); timer_ = this->create_wall_timer( @@ -84,8 +85,7 @@ void MissionLaneConverterNode::TimedStartupTrajectoryCallback() { if (!mission_lanes_available_once_) { // Empty trajectory for controller - autoware_auto_planning_msgs::msg::Trajectory trj_msg = - autoware_auto_planning_msgs::msg::Trajectory(); + autoware_planning_msgs::msg::Trajectory trj_msg = autoware_planning_msgs::msg::Trajectory(); // Frame id trj_msg.header.frame_id = local_map_frame_; @@ -125,19 +125,19 @@ void MissionLaneConverterNode::MissionLanesCallback_( } std::tuple< - autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, - autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + autoware_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> mission_to_trj = ConvertMissionToTrajectory(msg_mission); - autoware_auto_planning_msgs::msg::Trajectory trj_msg = std::get<0>(mission_to_trj); + autoware_planning_msgs::msg::Trajectory trj_msg = std::get<0>(mission_to_trj); visualization_msgs::msg::Marker trj_vis = std::get<1>(mission_to_trj); - autoware_auto_planning_msgs::msg::Path path_msg = std::get<2>(mission_to_trj); + autoware_planning_msgs::msg::Path path_msg = std::get<2>(mission_to_trj); visualization_msgs::msg::MarkerArray path_area = std::get<3>(mission_to_trj); - autoware_auto_planning_msgs::msg::Trajectory trj_msg_global = - TransformToGlobalFrame(trj_msg); - autoware_auto_planning_msgs::msg::Path path_msg_global = - TransformToGlobalFrame(path_msg); + autoware_planning_msgs::msg::Trajectory trj_msg_global = + TransformToGlobalFrame(trj_msg); + autoware_planning_msgs::msg::Path path_msg_global = + TransformToGlobalFrame(path_msg); visualization_msgs::msg::Marker trj_vis_global = GetGlobalTrjVisualization_(trj_msg_global); vis_trajectory_publisher_global_->publish(trj_vis_global); @@ -171,16 +171,15 @@ void MissionLaneConverterNode::MissionLanesCallback_( } std::tuple< - autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, - autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + autoware_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> MissionLaneConverterNode::ConvertMissionToTrajectory( const autoware_planning_msgs::msg::MissionLanesStamped & msg) { // Empty trajectory for controller - autoware_auto_planning_msgs::msg::Trajectory trj_msg = - autoware_auto_planning_msgs::msg::Trajectory(); + autoware_planning_msgs::msg::Trajectory trj_msg = autoware_planning_msgs::msg::Trajectory(); - autoware_auto_planning_msgs::msg::Path path_msg = autoware_auto_planning_msgs::msg::Path(); + autoware_planning_msgs::msg::Path path_msg = autoware_planning_msgs::msg::Path(); // Empty trajectory visualization message visualization_msgs::msg::Marker trj_vis, path_center_vis, path_left_vis, path_right_vis; @@ -290,9 +289,8 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( } void MissionLaneConverterNode::CreateMotionPlannerInput_( - autoware_auto_planning_msgs::msg::Trajectory & trj_msg, - autoware_auto_planning_msgs::msg::Path & path_msg, visualization_msgs::msg::Marker & trj_vis, - visualization_msgs::msg::Marker & path_vis, + autoware_planning_msgs::msg::Trajectory & trj_msg, autoware_planning_msgs::msg::Path & path_msg, + visualization_msgs::msg::Marker & trj_vis, visualization_msgs::msg::Marker & path_vis, const std::vector & centerline_mission_lane) { // Add a mission lane's centerline to the output trajectory and path messages @@ -373,11 +371,10 @@ void MissionLaneConverterNode::CreatePathBound_( } void MissionLaneConverterNode::AddPathPoint_( - autoware_auto_planning_msgs::msg::Path & pth_msg, const double x, const double y, - const double v_x) + autoware_planning_msgs::msg::Path & pth_msg, const double x, const double y, const double v_x) { // Add a trajectory point - pth_msg.points.push_back(autoware_auto_planning_msgs::msg::PathPoint()); + pth_msg.points.push_back(autoware_planning_msgs::msg::PathPoint()); // Fill trajectory points with meaningful data pth_msg.points.back().pose.position.x = x; @@ -390,11 +387,11 @@ void MissionLaneConverterNode::AddPathPoint_( } void MissionLaneConverterNode::AddTrajectoryPoint_( - autoware_auto_planning_msgs::msg::Trajectory & trj_msg, const double x, const double y, + autoware_planning_msgs::msg::Trajectory & trj_msg, const double x, const double y, const double v_x) { // Add a trajectory point - trj_msg.points.push_back(autoware_auto_planning_msgs::msg::TrajectoryPoint()); + trj_msg.points.push_back(autoware_planning_msgs::msg::TrajectoryPoint()); // Fill trajectory points with meaningful data trj_msg.points.back().pose.position.x = x; @@ -420,7 +417,7 @@ void MissionLaneConverterNode::AddPointVisualizationMarker_( } void MissionLaneConverterNode::AddHeadingToTrajectory_( - autoware_auto_planning_msgs::msg::Trajectory & trj_msg) + autoware_planning_msgs::msg::Trajectory & trj_msg) { std::vector points; @@ -549,7 +546,7 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) } // Convert the path area's bounds - if constexpr (std::is_same::value) { + if constexpr (std::is_same::value) { for (size_t ib = 0; ib < 2; ib++) { std::vector bound; if (ib == 0) @@ -575,7 +572,7 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) } // Add heading if type is a trajectory - if constexpr (std::is_same::value) + if constexpr (std::is_same::value) AddHeadingToTrajectory_(msg_output); } @@ -583,7 +580,7 @@ T MissionLaneConverterNode::TransformToGlobalFrame(const T & msg_input) } visualization_msgs::msg::Marker MissionLaneConverterNode::GetGlobalTrjVisualization_( - const autoware_auto_planning_msgs::msg::Trajectory & trj_msg) + const autoware_planning_msgs::msg::Trajectory & trj_msg) { // Empty trajectory visualization message visualization_msgs::msg::Marker trj_vis_global; @@ -608,3 +605,7 @@ visualization_msgs::msg::Marker MissionLaneConverterNode::GetGlobalTrjVisualizat return trj_vis_global; } } // namespace autoware::mapless_architecture + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mapless_architecture::MissionLaneConverterNode) diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp index 51d7178e445aa..30f33f6e34d02 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -3,6 +3,7 @@ #include "autoware/mission_lane_converter/mission_lane_converter_node.hpp" #include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point.hpp" @@ -11,7 +12,8 @@ namespace autoware::mapless_architecture int TestMissionToTrajectory() { - MissionLaneConverterNode mission_converter = MissionLaneConverterNode(); + rclcpp::NodeOptions options; + MissionLaneConverterNode mission_converter = MissionLaneConverterNode(options); autoware_planning_msgs::msg::MissionLanesStamped mission_msg; @@ -28,12 +30,12 @@ int TestMissionToTrajectory() // Get converted trajectory std::tuple< - autoware_auto_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, - autoware_auto_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + autoware_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> mission_to_trj = mission_converter.ConvertMissionToTrajectory(mission_msg); // Extract trajectory - autoware_auto_planning_msgs::msg::Trajectory trj_msg = std::get<0>(mission_to_trj); + autoware_planning_msgs::msg::Trajectory trj_msg = std::get<0>(mission_to_trj); EXPECT_EQ(trj_msg.points.back().pose.position.x, mission_msg.ego_lane.centerline.back().x); EXPECT_EQ(trj_msg.points.back().pose.position.y, mission_msg.ego_lane.centerline.back().y); From 77749eb0671b63588439f7e62cd9c39ea47054ca Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Tue, 9 Jul 2024 09:16:46 +0000 Subject: [PATCH 23/24] 3: Change the license --- .../include/autoware/hmi/hmi_node.hpp | 16 ++++++++++++++-- .../autoware_hmi/launch/hmi.launch.py | 16 ++++++++++++++-- .../autoware_hmi/src/hmi_node.cpp | 15 +++++++++++++-- .../autoware_hmi/test/gtest_main.cpp | 16 ++++++++++++++-- .../local_map_provider_node.hpp | 16 ++++++++++++++-- .../launch/local_map_provider.launch.py | 16 ++++++++++++++-- .../src/local_map_provider_node.cpp | 15 +++++++++++++-- .../test/gtest_main.cpp | 16 ++++++++++++++-- .../mission_planner_node.hpp | 16 ++++++++++++++-- .../launch/mission_planner.launch.py | 16 ++++++++++++++-- .../launch/mission_planner_compose.launch.py | 16 ++++++++++++++-- .../src/mission_planner_node.cpp | 16 ++++++++++++++-- .../test/gtest_main.cpp | 15 +++++++++++++-- .../test/include/test_mission_planner_core.hpp | 16 ++++++++++++++-- .../test/src/test_mission_planner_core.cpp | 15 +++++++++++++-- .../helper_functions.hpp | 14 +++++++++++++- .../src/helper_functions.cpp | 14 +++++++++++++- .../test/gtest_main.cpp | 14 +++++++++++++- .../mission_lane_converter_node.hpp | 16 ++++++++++++++-- .../launch/mission_lane_converter.launch.py | 16 ++++++++++++++-- .../src/mission_lane_converter_node.cpp | 15 +++++++++++++-- .../test/gtest_main.cpp | 16 ++++++++++++++-- .../include/test_mission_planner_converter.hpp | 15 +++++++++++++-- .../test/src/test_mission_planner_converter.cpp | 15 +++++++++++++-- 24 files changed, 326 insertions(+), 45 deletions(-) diff --git a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp index fc090a93cc3a4..aca44387c2d8a 100644 --- a/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp +++ b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 AUTOWARE__HMI__HMI_NODE_HPP_ #define AUTOWARE__HMI__HMI_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py index 7544a2d9f098e..83f207b302ca2 100644 --- a/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py +++ b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py @@ -1,5 +1,17 @@ -# Copyright 2024 driveblocks GmbH -# driveblocks proprietary license +# Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +#     http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from launch import LaunchDescription from launch_ros.actions import Node diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index 8e600dc95bfa6..c7050bc89c481 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -1,5 +1,16 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "autoware/hmi/hmi_node.hpp" diff --git a/planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp b/planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp index d885ea084b1a3..bfca1e4a8487b 100644 --- a/planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "gtest/gtest.h" int main(int argc, char ** argv) diff --git a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp index 5a8effd39f11d..fdb0fb5a168a8 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp +++ b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 AUTOWARE__LOCAL_MAP_PROVIDER__LOCAL_MAP_PROVIDER_NODE_HPP_ #define AUTOWARE__LOCAL_MAP_PROVIDER__LOCAL_MAP_PROVIDER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py index ccdbd6c1a9ea9..6bb4500cef7b0 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py +++ b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py @@ -1,5 +1,17 @@ -# Copyright 2024 driveblocks GmbH -# driveblocks proprietary license +# Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +#     http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from launch import LaunchDescription from launch_ros.actions import Node diff --git a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp index 726a396406f77..832efcdad2223 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -1,5 +1,16 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "autoware/local_map_provider/local_map_provider_node.hpp" diff --git a/planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp index d885ea084b1a3..bfca1e4a8487b 100644 --- a/planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "gtest/gtest.h" int main(int argc, char ** argv) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index 099d41e68c15b..da6dda6134007 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 AUTOWARE__LOCAL_MISSION_PLANNER__MISSION_PLANNER_NODE_HPP_ #define AUTOWARE__LOCAL_MISSION_PLANNER__MISSION_PLANNER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py index a6f56913832f2..7772b1793d860 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py @@ -1,5 +1,17 @@ -# Copyright 2024 driveblocks GmbH -# driveblocks proprietary license +# Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +# +# Licensed under the Apache 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 from ament_index_python.packages import get_package_share_directory diff --git a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py index fcf15de872045..f79bde9ab238d 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py @@ -1,5 +1,17 @@ -# Copyright 2024 driveblocks GmbH -# driveblocks proprietary license +# Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +#     http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + from pathlib import Path from ament_index_python.packages import get_package_share_directory diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index 3b17ff32361b2..edd338f20a319 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "autoware/local_mission_planner/mission_planner_node.hpp" #include "autoware/local_mission_planner_common/helper_functions.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp index 1158afd039a38..6d02f7c5cca38 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp @@ -1,5 +1,16 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "gtest/gtest.h" #include "include/test_mission_planner_core.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp index d75852cee7c70..12625e0794cb0 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 TEST_MISSION_PLANNER_CORE_HPP_ #define TEST_MISSION_PLANNER_CORE_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp index 424c7e562132c..07898e4cf22df 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -1,5 +1,16 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "autoware/local_mission_planner/mission_planner_node.hpp" #include "autoware/local_mission_planner_common/helper_functions.hpp" diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index 0d5fd8cdda31f..48a7aa4cf0568 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -1,5 +1,17 @@ // Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// +// Licensed under the Apache 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 AUTOWARE__LOCAL_MISSION_PLANNER_COMMON__HELPER_FUNCTIONS_HPP_ #define AUTOWARE__LOCAL_MISSION_PLANNER_COMMON__HELPER_FUNCTIONS_HPP_ diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index 7e0d1582cdffc..188b091360da8 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -1,5 +1,17 @@ // Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// +// Licensed under the Apache 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 "autoware/local_mission_planner_common/helper_functions.hpp" #include "lanelet2_core/geometry/Lanelet.h" diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp index 943b5400ca2c7..0bcd1efe7fcd6 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp @@ -1,5 +1,17 @@ // Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// +// Licensed under the Apache 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 "gtest/gtest.h" /** diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp index 209006541d0a6..7da7d9a5ff17a 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_ #define AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_ diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py index 3fecc4845e730..c65df5b118f27 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py +++ b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py @@ -1,5 +1,17 @@ -# Copyright 2024 driveblocks GmbH -# driveblocks proprietary license +# Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +# +# Licensed under the Apache 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 from ament_index_python.packages import get_package_share_directory diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index 1da3d197950eb..d0276e50886d6 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -1,5 +1,16 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "autoware/mission_lane_converter/mission_lane_converter_node.hpp" diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp index 4c906b0b06a44..d68ecb03bc8d4 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp @@ -1,5 +1,17 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "gtest/gtest.h" #include "include/test_mission_planner_converter.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp index 78aef5e313ccf..5166a2fd09b58 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp @@ -1,5 +1,16 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 TEST_MISSION_PLANNER_CONVERTER_HPP_ #define TEST_MISSION_PLANNER_CONVERTER_HPP_ diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp index 30f33f6e34d02..07a8f2172dc3d 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -1,5 +1,16 @@ -// Copyright 2024 driveblocks GmbH -// driveblocks proprietary license +// Copyright 2024 driveblocks GmbH, authors: Simon Eisenmann, Thomas Herrmann +// +// Licensed under the Apache 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 "autoware/mission_lane_converter/mission_lane_converter_node.hpp" #include "gtest/gtest.h" From ff9dfd8b8bc16033a59e8491bed9b5ead9b50e4a Mon Sep 17 00:00:00 2001 From: Simon Eisenmann Date: Tue, 9 Jul 2024 15:56:56 +0000 Subject: [PATCH 24/24] 6: Check TODOs/FIXMEs --- .../autoware_hmi/src/hmi_node.cpp | 4 +- .../mission_planner_node.hpp | 5 ++- .../param/mission_planner_default.yaml | 1 + .../src/mission_planner_node.cpp | 45 +++++++++++-------- .../helper_functions.hpp | 10 +++++ .../src/helper_functions.cpp | 11 ++++- .../mission_lane_converter_node.hpp | 4 ++ .../src/mission_lane_converter_node.cpp | 40 +++++++++++------ 8 files changed, 83 insertions(+), 37 deletions(-) diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp index c7050bc89c481..2ca2793e47a84 100644 --- a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -83,8 +83,8 @@ void HMINode::PublishMission_(std::string mission) missionMessage.mission_type = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_RIGHT; } - // TODO(simon.eisenmann@driveblocks.ai): Change deadline parameter - missionMessage.deadline = 1000; + missionMessage.deadline = 1000; // This parameter can be changed if needed (it will be set by the + // software in the future). mission_publisher_->publish(missionMessage); } diff --git a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp index da6dda6134007..e1a18ae3aaecd 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -248,8 +248,10 @@ class MissionPlannerNode : public rclcpp::Node Direction target_lane_ = stay; Direction mission_ = stay; int retry_attempts_ = 0; + int recenter_counter_ = 0; bool lane_change_trigger_success_ = true; Direction lane_change_direction_ = stay; + float deadline_target_lane_ = 1000; lanelet::BasicPoint2d goal_point_; std::vector ego_lane_; @@ -262,9 +264,10 @@ class MissionPlannerNode : public rclcpp::Node float projection_distance_on_goallane_; int retrigger_attempts_max_; std::string local_map_frame_; + int recenter_period_; // Unique ID for each marker - int centerline_marker_id_ = 0; + ID centerline_marker_id_; }; } // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml index a95d5d3db2dc0..68c9c453d6e8d 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml +++ b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml @@ -5,3 +5,4 @@ projection_distance_on_goallane: 20.0 # [m] projection distance of goal point retrigger_attempts_max: 10 # number of attempts for triggering a lane change local_map_frame: map # Identifier of local map frame. Currently, there is no way to set global ROS params https://github.com/ros2/ros2cli/issues/778 -> This param has to be set in the mission converter also! + recenter_period: 10 # recenter goal point after 10 odometry updates diff --git a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp index edd338f20a319..575fa07fa8c0f 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -93,7 +93,7 @@ MissionPlannerNode::MissionPlannerNode(const rclcpp::NodeOptions & options) tf_listener_ = std::make_unique(*tf_buffer_); // Set ros parameters (DEFAULT values will be overwritten by external - // parameter file if exists) + // parameter file) distance_to_centerline_threshold_ = declare_parameter("distance_to_centerline_threshold", 0.2); RCLCPP_INFO( @@ -113,6 +113,13 @@ MissionPlannerNode::MissionPlannerNode(const rclcpp::NodeOptions & options) local_map_frame_ = declare_parameter("local_map_frame", "map"); RCLCPP_INFO(this->get_logger(), "Local map frame identifier: %s", local_map_frame_.c_str()); + + recenter_period_ = declare_parameter("recenter_period", 10); + RCLCPP_INFO( + this->get_logger(), + "After this number of odometry updates the goal point (used for lane change) is recentered (on " + "the centerline): %d", + recenter_period_); } void MissionPlannerNode::CallbackLocalMapMessages_( @@ -233,9 +240,7 @@ void MissionPlannerNode::CallbackLocalMapMessages_( break; } - lanes.deadline_target_lane = - std::numeric_limits::infinity(); // TODO(simon.eisenmann@driveblocks.ai): - // Change this value + lanes.deadline_target_lane = deadline_target_lane_; // Create driving corridors and add them to the MissionLanesStamped message lanes.ego_lane = CreateDrivingCorridor(ego_lane_, converted_lanelets); @@ -308,14 +313,22 @@ void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry goal_point_.x() = target_pose.get_x(); goal_point_.y() = target_pose.get_y(); - // Re-center updated goal point to lie on centerline (to get rid of issues + // Recenter updated goal point to lie on centerline (to get rid of issues // with a less accurate odometry update which could lead to loosing the - // goal lane) - const lanelet::BasicPoint2d target_point_2d = RecenterGoalPoint(goal_point_, current_lanelets_); + // goal lane), recenter only after a certain number of steps (recenter_period_) to reduce the + // calling frequency + if (recenter_counter_ >= recenter_period_) { + const lanelet::BasicPoint2d target_point_2d = + RecenterGoalPoint(goal_point_, current_lanelets_); - // Overwrite goal point - goal_point_.x() = target_point_2d.x(); - goal_point_.y() = target_point_2d.y(); + // Overwrite goal point + goal_point_.x() = target_point_2d.x(); + goal_point_.y() = target_point_2d.y(); + + recenter_counter_ = 0; + } else { + recenter_counter_++; + } // --- Start of debug visualization // Create marker and publish it @@ -405,6 +418,8 @@ void MissionPlannerNode::CallbackMissionMessages_(const autoware_planning_msgs:: RCLCPP_INFO(this->get_logger(), "Mission does not match."); } + deadline_target_lane_ = msg.deadline; + return; } @@ -548,8 +563,7 @@ void MissionPlannerNode::CheckIfGoalPointShouldBeReset_( { // Check if goal point should be reset: If the x value of the goal point is // negative, then the point is behind the vehicle and must be therefore reset. - if (goal_point_.x() < 0 && mission_ != stay) { // TODO(simon.eisenmann@driveblocks.ai): Maybe - // remove condition mission_ != stay + if (goal_point_.x() < 0 && mission_ != stay) { // Find the index of the lanelet containing the goal point int goal_index = FindOccupiedLaneletID(converted_lanelets, goal_point_); // Returns -1 if no match @@ -763,12 +777,7 @@ void MissionPlannerNode::VisualizeCenterlineOfDrivingCorridor( centerline_marker.ns = "centerline"; // Unique ID - if (centerline_marker_id_ == std::numeric_limits::max()) { - // Handle overflow - centerline_marker_id_ = 0; - } else { - centerline_marker.id = centerline_marker_id_++; - } + centerline_marker.id = centerline_marker_id_.ReturnIDAndIncrement(); centerline_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; centerline_marker.action = visualization_msgs::msg::Marker::ADD; diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp index 48a7aa4cf0568..90c626e112017 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -66,6 +66,16 @@ class Pose2D double psi_; }; +class ID +{ +public: + ID() : value_(0) {} + unsigned int ReturnIDAndIncrement(); + +private: + unsigned int value_; +}; + /** * Represent a 2D pose (pose_prev) in a new origin / coordinate system, which * is given in relation to the previous coordinate system / origin diff --git a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp index 188b091360da8..a21e804b6bdb1 100644 --- a/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -229,8 +229,6 @@ std::vector GetRelevantAdjacentLanelets( ids_relevant_successors = ids_adjacent_lanelets; } - // FIXME: avoid that list is empty -> has to be -1 if no relevant lanelet - // exists, this fixes an issue that originates in the lanelet converter; should be fixed there! if (ids_relevant_successors.empty()) { ids_relevant_successors.push_back(-1); } @@ -664,4 +662,13 @@ void CalculatePredecessors(std::vector & lanelet_connections) } } +unsigned int ID::ReturnIDAndIncrement() +{ + if (value_ == std::numeric_limits::max()) { + RCLCPP_WARN(rclcpp::get_logger("ID"), "ID overflow"); + value_ = 0; // Reset value_ to 0 + } + return value_++; +} + } // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp index 7da7d9a5ff17a..813111b850ed0 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_ #define AUTOWARE__MISSION_LANE_CONVERTER__MISSION_LANE_CONVERTER_NODE_HPP_ +#include "autoware/local_mission_planner_common/helper_functions.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_planning_msgs/msg/mission_lanes_stamped.hpp" @@ -201,6 +202,9 @@ class MissionLaneConverterNode : public rclcpp::Node // ROS parameters float target_speed_; std::string local_map_frame_; + + // Unique ID for each marker + ID marker_id_; }; } // namespace autoware::mapless_architecture diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp index d0276e50886d6..7f76ff028838c 100644 --- a/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -161,7 +161,6 @@ void MissionLaneConverterNode::MissionLanesCallback_( path_publisher_->publish(path_msg); path_publisher_global_->publish(path_msg_global); - // TODO(thomas.herrmann@driveblocks.ai): outsource this to a separate method // Clear all markers in scene visualization_msgs::msg::Marker msg_marker; msg_marker.header = msg_mission.header; @@ -239,8 +238,12 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( trj_msg, path_msg, trj_vis, path_center_vis, msg.ego_lane.centerline); // Fill path bounds left and right - CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); - CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); + CreatePathBound_( + path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, + marker_id_.ReturnIDAndIncrement()); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, + marker_id_.ReturnIDAndIncrement()); break; case -1: // Lane change to the left @@ -249,8 +252,11 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( // Fill path bounds left and right CreatePathBound_( - path_msg.left_bound, path_left_vis, msg.drivable_lanes_left[0].bound_left, 1); - CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); + path_msg.left_bound, path_left_vis, msg.drivable_lanes_left[0].bound_left, + marker_id_.ReturnIDAndIncrement()); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, + marker_id_.ReturnIDAndIncrement()); break; case 1: // Lane change to the right @@ -258,9 +264,12 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right[0].centerline); // Fill path bounds left and right - CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); CreatePathBound_( - path_msg.right_bound, path_right_vis, msg.drivable_lanes_right[0].bound_right, 2); + path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, + marker_id_.ReturnIDAndIncrement()); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.drivable_lanes_right[0].bound_right, + marker_id_.ReturnIDAndIncrement()); break; case -2: // Take exit left @@ -269,8 +278,11 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( // Fill path bounds left and right CreatePathBound_( - path_msg.left_bound, path_left_vis, msg.drivable_lanes_left.back().bound_left, 1); - CreatePathBound_(path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, 2); + path_msg.left_bound, path_left_vis, msg.drivable_lanes_left.back().bound_left, + marker_id_.ReturnIDAndIncrement()); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.ego_lane.bound_right, + marker_id_.ReturnIDAndIncrement()); break; case 2: // Take exit right @@ -278,9 +290,12 @@ MissionLaneConverterNode::ConvertMissionToTrajectory( trj_msg, path_msg, trj_vis, path_center_vis, msg.drivable_lanes_right.back().centerline); // Fill path bounds left and right - CreatePathBound_(path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, 1); CreatePathBound_( - path_msg.right_bound, path_right_vis, msg.drivable_lanes_right.back().bound_right, 2); + path_msg.left_bound, path_left_vis, msg.ego_lane.bound_left, + marker_id_.ReturnIDAndIncrement()); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.drivable_lanes_right.back().bound_right, + marker_id_.ReturnIDAndIncrement()); break; default: @@ -374,7 +389,6 @@ void MissionLaneConverterNode::CreatePathBound_( } // Add last added point to a marker message for debugging - // FIXME: probably no unique ids for multiple calls? AddPointVisualizationMarker_(path_vis, bound_path.back().x, bound_path.back().y, id_marker); } @@ -456,8 +470,6 @@ void MissionLaneConverterNode::AddHeadingToTrajectory_( return; } -// TODO(thomas.herrmann@driveblocks.ai): store the latest odometry message here and then re-use in -// the output conversion void MissionLaneConverterNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg) { // Store current odometry information