diff --git a/planning/.pages b/planning/.pages index d072126b506e8..f4abad35aef25 100644 --- a/planning/.pages +++ b/planning/.pages @@ -39,7 +39,13 @@ 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': - 'About Path Optimizer': planning/autoware_path_optimizer diff --git a/planning/mapless_architecture/README.md b/planning/mapless_architecture/README.md new file mode 100644 index 0000000000000..c987dc8eb309e --- /dev/null +++ b/planning/mapless_architecture/README.md @@ -0,0 +1,35 @@ +# 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: + +- **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 + +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 +``` + +## 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. diff --git a/planning/mapless_architecture/autoware_hmi/CMakeLists.txt b/planning/mapless_architecture/autoware_hmi/CMakeLists.txt new file mode 100644 index 0000000000000..9de77f224e8b6 --- /dev/null +++ b/planning/mapless_architecture/autoware_hmi/CMakeLists.txt @@ -0,0 +1,57 @@ +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) +find_package(rclcpp_components REQUIRED) +ament_auto_find_build_dependencies() +autoware_package() + +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 + $ + $) + +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) + +# Install the target library +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# Install the launch directory +install(DIRECTORY +launch +DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +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() + +# 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 new file mode 100644 index 0000000000000..aa74343559f6f --- /dev/null +++ b/planning/mapless_architecture/autoware_hmi/Readme.md @@ -0,0 +1,29 @@ +# 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: + +```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 new file mode 100644 index 0000000000000..aca44387c2d8a --- /dev/null +++ b/planning/mapless_architecture/autoware_hmi/include/autoware/hmi/hmi_node.hpp @@ -0,0 +1,65 @@ +// 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_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_planning_msgs/msg/mission.hpp" + +#include +#include + +namespace autoware::mapless_architecture +{ + +/** + * 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. + */ + explicit HMINode(const rclcpp::NodeOptions & options); + +private: + /** + * @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); + + // Declare ROS2 publisher and subscriber + + rclcpp::Publisher::SharedPtr mission_publisher_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; +}; +} // namespace autoware::mapless_architecture + +#endif // 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 new file mode 100644 index 0000000000000..83f207b302ca2 --- /dev/null +++ b/planning/mapless_architecture/autoware_hmi/launch/hmi.launch.py @@ -0,0 +1,35 @@ +# 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 + + +def generate_launch_description(): + return LaunchDescription( + [ + # hmi executable + Node( + package="autoware_hmi", + executable="autoware_hmi_exe", + name="autoware_hmi", + namespace="mapless_architecture", + remappings=[ + ("hmi_node/output/mission", "hmi_node/output/mission"), + ], + parameters=[], + output="screen", + ), + ] + ) diff --git a/planning/mapless_architecture/autoware_hmi/package.xml b/planning/mapless_architecture/autoware_hmi/package.xml new file mode 100644 index 0000000000000..274112c5a27d8 --- /dev/null +++ b/planning/mapless_architecture/autoware_hmi/package.xml @@ -0,0 +1,24 @@ + + + + autoware_hmi + 0.0.1 + HMI + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + autoware_planning_msgs + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp new file mode 100644 index 0000000000000..2ca2793e47a84 --- /dev/null +++ b/planning/mapless_architecture/autoware_hmi/src/hmi_node.cpp @@ -0,0 +1,95 @@ +// 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" + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_planning_msgs/msg/mission.hpp" + +namespace autoware::mapless_architecture +{ +using std::placeholders::_1; + +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) + // 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) +{ + autoware_planning_msgs::msg::Mission missionMessage; + if (mission == "LANE_KEEP") { + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_KEEP; + } else if (mission == "LANE_CHANGE_LEFT") { + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::LANE_CHANGE_LEFT; + } else if (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 = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_LEFT; + } else if (mission == "TAKE_NEXT_EXIT_RIGHT") { + missionMessage.mission_type = autoware_planning_msgs::msg::Mission::TAKE_NEXT_EXIT_RIGHT; + } + + missionMessage.deadline = 1000; // This parameter can be changed if needed (it will be set by the + // software in the future). + + 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_hmi/test/gtest_main.cpp b/planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp new file mode 100644 index 0000000000000..bfca1e4a8487b --- /dev/null +++ b/planning/mapless_architecture/autoware_hmi/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// 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) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt b/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt new file mode 100644 index 0000000000000..9e3cbee014f49 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_map_provider/CMakeLists.txt @@ -0,0 +1,58 @@ +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) +find_package(rclcpp_components REQUIRED) +ament_auto_find_build_dependencies() +autoware_package() + +# 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 + $ + $) + +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) + +# Install the target library +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# Install the launch directory +install(DIRECTORY +launch +DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +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() + +# 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 new file mode 100644 index 0000000000000..d5d39c74ef958 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_map_provider/Readme.md @@ -0,0 +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 new file mode 100644 index 0000000000000..fdb0fb5a168a8 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_map_provider/include/autoware/local_map_provider/local_map_provider_node.hpp @@ -0,0 +1,56 @@ +// 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_ + +#include "rclcpp/rclcpp.hpp" + +#include "autoware_planning_msgs/msg/local_map.hpp" +#include "autoware_planning_msgs/msg/road_segments.hpp" + +namespace autoware::mapless_architecture +{ + +/** + * 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. + */ + explicit LocalMapProviderNode(const rclcpp::NodeOptions & options); + +private: + /** + * @brief The callback for the RoadSegments messages. + * + * @param msg The autoware_planning_msgs::msg::RoadSegments message. + */ + void CallbackRoadSegmentsMessages_(const autoware_planning_msgs::msg::RoadSegments & msg); + + // Declare ROS2 publisher and subscriber + + rclcpp::Publisher::SharedPtr map_publisher_; + + rclcpp::Subscription::SharedPtr road_subscriber_; +}; +} // namespace autoware::mapless_architecture + +#endif // 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 new file mode 100644 index 0000000000000..6bb4500cef7b0 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_map_provider/launch/local_map_provider.launch.py @@ -0,0 +1,42 @@ +# 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 + + +def generate_launch_description(): + return LaunchDescription( + [ + # autoware_local_map_provider executable + Node( + package="autoware_local_map_provider", + executable="autoware_local_map_provider_exe", + name="autoware_local_map_provider", + namespace="mapless_architecture", + 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/autoware_local_map_provider/package.xml b/planning/mapless_architecture/autoware_local_map_provider/package.xml new file mode 100644 index 0000000000000..c5fa7680f9f1f --- /dev/null +++ b/planning/mapless_architecture/autoware_local_map_provider/package.xml @@ -0,0 +1,25 @@ + + + + autoware_local_map_provider + 0.0.1 + local_map_provider + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + autoware_planning_msgs + lib_mission_planner + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..832efcdad2223 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_map_provider/src/local_map_provider_node.cpp @@ -0,0 +1,58 @@ +// 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" + +#include "rclcpp/rclcpp.hpp" + +namespace autoware::mapless_architecture +{ +using std::placeholders::_1; + +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) + // 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 autoware_planning_msgs::msg::RoadSegments & msg) +{ + autoware_planning_msgs::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 +} +} // 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_map_provider/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp new file mode 100644 index 0000000000000..bfca1e4a8487b --- /dev/null +++ b/planning/mapless_architecture/autoware_local_map_provider/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// 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) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt new file mode 100644 index 0000000000000..2e5f431086536 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/CMakeLists.txt @@ -0,0 +1,60 @@ +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) +find_package(rclcpp_components REQUIRED) +ament_auto_find_build_dependencies() +autoware_package() + +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 + $ + $) + +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) + +# Install the target library +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# Install the launch and param directories +install(DIRECTORY + launch + param + DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +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_core.cpp + src/mission_planner_node.cpp) + + ament_lint_auto_find_test_dependencies() +endif() + +# 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 new file mode 100644 index 0000000000000..b7d98ae933556 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/Readme.md @@ -0,0 +1,31 @@ +# 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. + +## 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 new file mode 100644 index 0000000000000..e1a18ae3aaecd --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/include/autoware/local_mission_planner/mission_planner_node.hpp @@ -0,0 +1,274 @@ +// 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_ + +#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 "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 + +namespace autoware::mapless_architecture +{ + +// 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; + +// Define Lanes +struct Lanes +{ + std::vector ego; + std::vector> left; + std::vector> right; +}; + +/** + * Node for mission planner. + */ +class MissionPlannerNode : public rclcpp::Node +{ +public: + explicit MissionPlannerNode(const rclcpp::NodeOptions & options); + + /** + * @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 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 The callback for the Mission messages. + * + * @param msg The autoware_planning_msgs::msg::Mission message. + */ + void CallbackMissionMessages_(const autoware_planning_msgs::msg::Mission & msg); + + /** + * @brief The callback for the LocalMap messages. + * + * @param msg The autoware_planning_msgs::msg::LocalMap message. + */ + void CallbackLocalMapMessages_(const autoware_planning_msgs::msg::LocalMap & msg); + + /** + * @brief Convert RoadSegments into lanelets. + * + * @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 autoware_planning_msgs::msg::RoadSegments & msg, + std::vector & out_lanelets, + std::vector & out_lanelet_connections); + + /** + * @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( + const autoware_planning_msgs::msg::RoadSegments & msg, + const std::vector & converted_lanelets); + + /** + * @brief Function for the visualization of the centerline of a driving corridor. + * + * @param msg The autoware_planning_msgs::msg::RoadSegments message. + * @param driving_corridor The considered driving corridor for which the centerline is visualized. + */ + void VisualizeCenterlineOfDrivingCorridor( + const autoware_planning_msgs::msg::RoadSegments & msg, + const autoware_planning_msgs::msg::DrivingCorridor & driving_corridor); + +private: + /** + * @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); + + // 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_; + + // Initialize some variables + Pose2D pose_prev_; + bool pose_prev_init_ = false; + bool b_input_odom_frame_error_ = false; + bool received_motion_update_once_ = false; + 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_; + 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_; + std::string local_map_frame_; + int recenter_period_; + + // Unique ID for each marker + ID centerline_marker_id_; +}; +} // namespace autoware::mapless_architecture + +#endif // 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 new file mode 100644 index 0000000000000..7772b1793d860 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner.launch.py @@ -0,0 +1,72 @@ +# 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 +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("autoware_local_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="autoware_local_mission_planner", + executable="autoware_local_mission_planner_exe", + name="autoware_local_mission_planner", + namespace="mapless_architecture", + 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", + ), + ] + ) 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 new file mode 100644 index 0000000000000..f79bde9ab238d --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/launch/mission_planner_compose.launch.py @@ -0,0 +1,72 @@ +# 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 +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( + "autoware_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("autoware_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_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 + ) + + 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_map_provider]) diff --git a/planning/mapless_architecture/autoware_local_mission_planner/package.xml b/planning/mapless_architecture/autoware_local_mission_planner/package.xml new file mode 100644 index 0000000000000..1e81bf9998f7d --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/package.xml @@ -0,0 +1,32 @@ + + + + autoware_local_mission_planner + 0.0.1 + Mission Planner + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + autoware_local_mission_planner_common + autoware_planning_msgs + builtin_interfaces + geometry_msgs + lanelet2_core + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..68c9c453d6e8d --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/param/mission_planner_default.yaml @@ -0,0 +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 + 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 new file mode 100644 index 0000000000000..575fa07fa8c0f --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/src/mission_planner_node.cpp @@ -0,0 +1,811 @@ +// 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" +#include "lanelet2_core/LaneletMap.h" +#include "lanelet2_core/geometry/Lanelet.h" +#include "lanelet2_core/geometry/LineString.h" +#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 "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 + +namespace autoware::mapless_architecture +{ +using std::placeholders::_1; + +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) + // 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) + 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_); + + 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_( + const autoware_planning_msgs::msg::LocalMap & msg) +{ + // Used for output + std::vector lanelet_connections; + std::vector converted_lanelets; + + ConvertInput2LaneletFormat(msg.road_segments, converted_lanelets, lanelet_connections); + 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 = 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 + 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(); + + // 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 = deadline_target_lane_; + + // Create driving corridors and add them to the MissionLanesStamped message + 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 = CreateDrivingCorridor(lane, converted_lanelets); + lanes.drivable_lanes_left.push_back(driving_corridor); + VisualizeCenterlineOfDrivingCorridor(msg.road_segments, driving_corridor); + } + } + + if (!right_lanes.empty()) { + for (const std::vector & lane : right_lanes) { + driving_corridor = CreateDrivingCorridor(lane, converted_lanelets); + lanes.drivable_lanes_right.push_back(driving_corridor); + VisualizeCenterlineOfDrivingCorridor(msg.road_segments, driving_corridor); + } + } + + // Publish MissionLanesStamped message + missionLanesStampedPublisher_->publish(lanes); +} + +void MissionPlannerNode::CallbackOdometryMessages_(const nav_msgs::msg::Odometry & msg) +{ + // Construct raw odometry pose + 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 + // odometry signal stems from the GNSS (and is therefore valid in the odom + // frame) + 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 <%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; + } + } + + // Calculate yaw for received pose + 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( + 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 + // 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(); + + // 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), 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(); + + recenter_counter_ = 0; + } else { + recenter_counter_++; + } + + // --- 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(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; + + return; +} + +void MissionPlannerNode::CallbackMissionMessages_(const autoware_planning_msgs::msg::Mission & msg) +{ + // Initialize variables + lane_change_trigger_success_ = false; + retry_attempts_ = 0; + + switch (msg.mission_type) { + case autoware_planning_msgs::msg::Mission::LANE_KEEP: + // Keep the lane + mission_ = stay; + target_lane_ = stay; + break; + 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 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 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 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 + break; + default: + // Nothing happens if mission does not match! + RCLCPP_INFO(this->get_logger(), "Mission does not match."); + } + + deadline_target_lane_ = msg.deadline; + + 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_); + } +} + +// 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 = + 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 = 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 = + 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, 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 = + 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, 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; +} + +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 = FindOccupiedLaneletID(converted_lanelets, goal_point); // Returns -1 if no match + + if (goal_index >= 0) { // Check if -1 + std::vector> goal_lane = 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; +} + +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) { + // Find the index of the lanelet containing the goal point + 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( + 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 autoware_planning_msgs::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(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_segment_id.size(); i++) { + out_lanelet_connections[idx_segment].neighbor_lanelet_ids.push_back( + msg.segments[idx_segment].neighboring_segment_id[i]); + } + + // Write lanelet successors + 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_segment_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 & 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()) { + segment_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 + CalculatePredecessors(out_lanelet_connections); + + return; +} + +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 = + 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."); + } + + // 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 + centerline_marker.id = centerline_marker_id_.ReturnIDAndIncrement(); + + 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 + +#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/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp new file mode 100644 index 0000000000000..6d02f7c5cca38 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/gtest_main.cpp @@ -0,0 +1,69 @@ +// 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" +#include "rclcpp/rclcpp.hpp" + +namespace autoware::mapless_architecture +{ + +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(); +} +} // 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 new file mode 100644 index 0000000000000..12625e0794cb0 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/include/test_mission_planner_core.hpp @@ -0,0 +1,90 @@ +// 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_ + +namespace autoware::mapless_architecture +{ + +/** + * @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(); + +} // 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 new file mode 100644 index 0000000000000..07898e4cf22df --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner/test/src/test_mission_planner_core.cpp @@ -0,0 +1,606 @@ +// 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" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/pose.hpp" + +#include +#include + +namespace autoware::mapless_architecture +{ +// Note: Lanelets and Segments are basically the same! + +autoware_planning_msgs::msg::RoadSegments CreateSegments() +{ + // Local variables + const int n_segments = 3; + + // Fill lanelet2 message + 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"; + + message.segments[0].id = 0; + message.segments[1].id = 1; + message.segments[2].id = 2; + + 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.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.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; +} + +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 + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // 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 segments + autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); + + // Initialize MissionPlannerNode + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // Convert road model + std::vector lanelet_connections; + std::vector 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 + // 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 segments + autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); + + // Initialize MissionPlannerNode + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // Convert road model + std::vector lanelet_connections; + std::vector lanelets; + + 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); + + // 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; +} + +autoware_planning_msgs::msg::RoadSegments GetTestRoadModelForRecenterTests() +{ + // local variables + const int n_segments = 2; + + // Fill lanelet2 message + 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"; + + message.segments[0].id = 0; + message.segments[1].id = 1; + + 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.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.segments[0].neighboring_segment_id = neighboring_ids; + std::vector neighboring_ids_2 = {}; + message.segments[1].neighboring_segment_id = neighboring_ids_2; + + return message; +} + +int TestRecenterGoalpoint() +{ + // Create a mission planner + rclcpp::NodeOptions options; + MissionPlannerNode mission_planner = MissionPlannerNode(options); + + // Get a local road model for testing + autoware_planning_msgs::msg::RoadSegments road_segments = GetTestRoadModelForRecenterTests(); + + // Used for the output + std::vector lanelet_connections; + std::vector converted_lanelets; + + 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 + lanelet::BasicPoint2d goal_point(0.0, 0.0); + 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); + + // 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 = 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 = 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 = 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 = 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 segments + autoware_planning_msgs::msg::RoadSegments road_segments = CreateSegments(); + + autoware_planning_msgs::msg::LocalMap local_map; + local_map.road_segments = road_segments; + + // Initialize MissionPlannerNode + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // Convert road model + std::vector lanelet_connections; + std::vector lanelets; + + 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 + 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 + 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 = autoware_planning_msgs::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_segments = 2; + + // Fill lanelet2 message + 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"; + + message.segments[0].id = 0; + message.segments[1].id = 1; + + 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.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.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 + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // Output + std::vector lanelet_connections; + std::vector converted_lanelets; + + MissionPlanner.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 + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // 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 + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // Create empty message + autoware_planning_msgs::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 = 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 + rclcpp::NodeOptions options; + MissionPlannerNode MissionPlanner = MissionPlannerNode(options); + + // Call function which is tested + autoware_planning_msgs::msg::DrivingCorridor driving_corridor = + 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; +} +} // 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 new file mode 100644 index 0000000000000..ddef091ae0f89 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_local_mission_planner_common) + +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) +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 + lanelet2_core + autoware_planning_msgs + visualization_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 + lanelet2_core + autoware_planning_msgs + visualization_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 +) + +# Testing +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 + 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_local_mission_planner_common/Readme.md b/planning/mapless_architecture/autoware_local_mission_planner_common/Readme.md new file mode 100644 index 0000000000000..898c81d39e7d3 --- /dev/null +++ b/planning/mapless_architecture/autoware_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/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 new file mode 100644 index 0000000000000..90c626e112017 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/include/autoware/local_mission_planner_common/helper_functions.hpp @@ -0,0 +1,373 @@ +// Copyright 2024 driveblocks GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//      http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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_ + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/Geometry" +#include "lanelet2_core/primitives/Lanelet.h" +#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 + +namespace autoware::mapless_architecture +{ + +/** + * @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_; +}; + +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 + * (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 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 (left or right). + * + */ +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); + +/** + * @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 new file mode 100644 index 0000000000000..f7537e8ad6250 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/package.xml @@ -0,0 +1,26 @@ + + + + autoware_local_mission_planner_common + 0.0.0 + Library containing helper functions for the different nodes. + driveblocks + driveblocks proprietary license + + autoware_cmake + + autoware_planning_msgs + geometry_msgs + lanelet2_core + tf2 + tf2_geometry_msgs + tf2_ros + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..a21e804b6bdb1 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/src/helper_functions.cpp @@ -0,0 +1,674 @@ +// Copyright 2024 driveblocks GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//      http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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" +#include "rclcpp/rclcpp.hpp" + +#include "autoware_planning_msgs/msg/road_segments.hpp" + +namespace autoware::mapless_architecture +{ + +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; +} + +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; + } + + 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); +} + +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}; + } + } +} + +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_local_mission_planner_common/test/gtest_main.cpp b/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp new file mode 100644 index 0000000000000..0bcd1efe7fcd6 --- /dev/null +++ b/planning/mapless_architecture/autoware_local_mission_planner_common/test/gtest_main.cpp @@ -0,0 +1,25 @@ +// Copyright 2024 driveblocks GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//      http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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" + +/** + * Test helper functions + */ + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt b/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt new file mode 100644 index 0000000000000..0f76de9b43e89 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/CMakeLists.txt @@ -0,0 +1,61 @@ +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) +find_package(rclcpp_components REQUIRED) +ament_auto_find_build_dependencies() +autoware_package() + +# 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 + $ + $) + +# Specify required C and C++ standards +target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) + +# Install the target library +install(TARGETS + ${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}) + +# Install the launch and param directories +install(DIRECTORY + launch + param + DESTINATION share/${PROJECT_NAME}) + +# --- SPECIFY TESTS --- +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() + +# 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 new file mode 100644 index 0000000000000..26956e2d110a1 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/Readme.md @@ -0,0 +1,25 @@ +# Mission Lane Converter + +Converts the selected mission lane to an autoware trajectory. + +## Input topics + +| 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 new file mode 100644 index 0000000000000..813111b850ed0 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/include/autoware/mission_lane_converter/mission_lane_converter_node.hpp @@ -0,0 +1,211 @@ +// 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_ + +#include "autoware/local_mission_planner_common/helper_functions.hpp" +#include "rclcpp/rclcpp.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" + +#include +#include +#include + +namespace autoware::mapless_architecture +{ + +/** + * Node to convert the mission lane to an autoware trajectory type. + */ +class MissionLaneConverterNode : public rclcpp::Node +{ +public: + 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_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: + /** + * @brief Computes a trajectory based on the mission planner input. + * + * @param msg Mission lanes from the mission planner module + */ + void MissionLanesCallback_(const autoware_planning_msgs::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_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_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(); + + /** + *@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_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_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); + + /** + * @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_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 + */ + 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_planning_msgs::msg::Trajectory & trj_msg); + + // 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 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 + 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_; + std::string local_map_frame_; + + // Unique ID for each marker + ID marker_id_; +}; +} // namespace autoware::mapless_architecture + +#endif // 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 new file mode 100644 index 0000000000000..c65df5b118f27 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/launch/mission_lane_converter.launch.py @@ -0,0 +1,79 @@ +# 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 +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("autoware_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="autoware_mission_lane_converter", + executable="autoware_mission_lane_converter_exe", + name="autoware_mission_lane_converter", + namespace="mapless_architecture", + 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/autoware_mission_lane_converter/package.xml b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml new file mode 100644 index 0000000000000..cff328dd6cc18 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/package.xml @@ -0,0 +1,28 @@ + + + + autoware_mission_lane_converter + 0.0.1 + Mission Lane Converter + driveblocks + driveblocks proprietary license + + autoware_cmake + + ros2launch + + autoware_local_mission_planner_common + autoware_planning_msgs + geometry_msgs + rclcpp + rclcpp_components + tf2_geometry_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 new file mode 100644 index 0000000000000..294bc38e830c9 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/param/mission_lane_converter_default.yaml @@ -0,0 +1,5 @@ +/mission_planner: + 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 new file mode 100644 index 0000000000000..7f76ff028838c --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/src/mission_lane_converter_node.cpp @@ -0,0 +1,634 @@ +// 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 "autoware/local_mission_planner_common/helper_functions.hpp" +#include "rclcpp/rclcpp.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 + +#include +#include + +namespace autoware::mapless_architecture +{ +using std::placeholders::_1; + +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) + // 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, 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( + "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_planning_msgs::msg::Trajectory trj_msg = autoware_planning_msgs::msg::Trajectory(); + + // 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++) { + 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 autoware_planning_msgs::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_planning_msgs::msg::Trajectory, visualization_msgs::msg::Marker, + autoware_planning_msgs::msg::Path, visualization_msgs::msg::MarkerArray> + mission_to_trj = ConvertMissionToTrajectory(msg_mission); + + 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_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_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); + + // 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); + + // 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_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_planning_msgs::msg::Trajectory trj_msg = autoware_planning_msgs::msg::Trajectory(); + + 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; + 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; + trj_msg.header.frame_id = local_map_frame_; + path_msg.header.frame_id = local_map_frame_; + + 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, + 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 + 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, + 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 + 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, + 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 + 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, + 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 + 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, + marker_id_.ReturnIDAndIncrement()); + CreatePathBound_( + path_msg.right_bound, path_right_vis, msg.drivable_lanes_right.back().bound_right, + marker_id_.ReturnIDAndIncrement()); + 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_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 + 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 + AddPointVisualizationMarker_(path_vis, bound_path.back().x, bound_path.back().y, id_marker); + } + + return; +} + +void MissionLaneConverterNode::AddPathPoint_( + 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_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_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_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_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; +} + +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 = local_map_frame_; + + // Construct raw odometry pose + 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 (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; + } + } + + 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_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; +} +} // 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/gtest_main.cpp b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp new file mode 100644 index 0000000000000..d68ecb03bc8d4 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/gtest_main.cpp @@ -0,0 +1,35 @@ +// 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" + +namespace autoware::mapless_architecture +{ + +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(); +} + +} // namespace autoware::mapless_architecture 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 new file mode 100644 index 0000000000000..5166a2fd09b58 --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/include/test_mission_planner_converter.hpp @@ -0,0 +1,30 @@ +// 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_ + +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 new file mode 100644 index 0000000000000..07a8f2172dc3d --- /dev/null +++ b/planning/mapless_architecture/autoware_mission_lane_converter/test/src/test_mission_planner_converter.cpp @@ -0,0 +1,128 @@ +// 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" +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/point.hpp" + +namespace autoware::mapless_architecture +{ + +int TestMissionToTrajectory() +{ + rclcpp::NodeOptions options; + MissionLaneConverterNode mission_converter = MissionLaneConverterNode(options); + + 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 = autoware_planning_msgs::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_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_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(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; + 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(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; + 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; +} +} // namespace autoware::mapless_architecture 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 + + +