feat: add mission_planner package (autowarefoundation#40)

* release v0.4.0 * Fix routing from crosswalk (autowarefoundation#767) Signed-off-by: Daisuke Nishimatsu <border_goldenmarket@yahoo.co.jp> * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit 5eba353. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Rename launch files to launch.xml (autowarefoundation#28) * port mission_planner to ROS2 (autowarefoundation#56) * port mission_planner to ROS2 Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * fix QoS for publishers Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Add geometry2 to repos (autowarefoundation#76) * add geometry2 package temporarily until new release Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * trigger-ci Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add tf2 dependency to the packages that use tf2_geometry_msgs Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "Add geometry2 to repos (autowarefoundation#76)" (autowarefoundation#96) * Revert "Add geometry2 to repos (autowarefoundation#76)" This reverts commit 61defd0. * Re-add tf2 dependencies * Revert "Re-add tf2 dependencies" This reverts commit e23b0c8b0826cf9518924d33349f9de34b4975df. * Debug CI pipeline * Revert "Debug CI pipeline" This reverts commit 58f1eba550360d82c08230552abfb64b33b23e0f. * Explicitly install known versions of the geometry packages * No need to skip tf2 packages anymore Co-authored-by: Esteve Fernandez <esteve@apache.org> * Rename h files to hpp (autowarefoundation#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * fixing trasient_local in ROS2 packages (autowarefoundation#160) * Add linters to mission_planner (autowarefoundation#156) * Added linters to mission_planner * Removed duplicate dependencies * Only add the cppcheck linter * Added linters to CMake * Ros2 v0.8.0 mission planner (autowarefoundation#278) * add use_sim-time option (autowarefoundation#454) * Fix rolling build errors (autowarefoundation#1225) * Add missing include files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Replace rclcpp::Duration Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use reference for exceptions Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use from_seconds Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Unify Apache-2.0 license name (autowarefoundation#1242) * Make planning modules components (autowarefoundation#1263) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Fix compiler warnings (autowarefoundation#1837) * Fix -Wunused-private-field Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wunused-variable Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wformat-security Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Winvalid-constexpr Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wdelete-non-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wdelete-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Winconsistent-missing-override Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wrange-loop-construct Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix "invalid application of 'sizeof' to an incomplete type" Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore -Wgnu-anonymous-struct and -Wnested-anon-types Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix lint Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore -Wno-deprecated-declarations in CUDA-related packages Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Docs/mission planner (autowarefoundation#1952) * add doc * update docs * Invoke code formatter at pre-commit (autowarefoundation#1935) * Run ament_uncrustify at pre-commit * Reformat existing files * Fix copyright and cpplint errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> * update mission planner doc (autowarefoundation#2044) * update mission planner doc * fix typo * Update planning/mission_planning/mission_planner/mission_planner-design.md Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * add pull over/out module (autowarefoundation#2147) * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * rename to README.md (autowarefoundation#550) * rename to README.md * dealt with new auto_msgs format Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> * port mission planner (autowarefoundation#538) * port lanelet2 msg Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * port route Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * fix precommit Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * Use route_handler package in mission_planner (autowarefoundation#579) * [autoware_vehicle_rviz_plugin/route_handler/simple_planning_simulator]fix some packages (autowarefoundation#606) * fix console meter * fix velocity_history * fix route handler * change topic name * Modify readme for mission planner (autowarefoundation#714) * modify readme for mission planner Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * fix document Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp> * fix readme Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Esteve Fernandez <esteve@apache.org> Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: kyoichi sugahara <81.s.kyo.19@gmail.com> Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
TakumiKozaka-T4 · Dec 7, 2021 · 70ec2e2 · 70ec2e2
1 parent eb47ec5
commit 70ec2e2
Show file tree

Hide file tree

Showing 14 changed files with 1,031 additions and 0 deletions.
diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt
@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.5)
+project(mission_planner)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(mission_planner_node SHARED
+  lib/mission_planner_base.cpp
+  src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
+  src/mission_planner_lanelet2/utility_functions.cpp
+)
+
+rclcpp_components_register_node(mission_planner_node
+  PLUGIN "mission_planner::MissionPlannerLanelet2"
+  EXECUTABLE mission_planner
+)
+
+ament_auto_add_library(goal_pose_visualizer_node SHARED
+  src/goal_pose_visualizer/goal_pose_visualizer.cpp
+)
+
+rclcpp_components_register_node(goal_pose_visualizer_node
+  PLUGIN "mission_planner::GoalPoseVisualizer"
+  EXECUTABLE goal_pose_visualizer
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md
@@ -0,0 +1,149 @@
+# Mission Planner
+
+## Purpose
+
+`Mission Planner` calculates a route that navigates from the current ego pose to the goal pose following the given check points.
+The route is made of a sequence of lanes on a static map.
+Dynamic objects (e.g. pedestrians and other vehicles) and dynamic map information (e.g. road construction which blocks some lanes) are not considered during route planning.
+Therefore, the output topic is only published when the goal pose or check points are given and will be latched until the new goal pose or check points are given.
+
+The core implementation does not depend on a map format.
+In current Autoware.universe, only Lanelet2 map format is supported.
+
+## Inputs / Outputs
+
+### input
+
+| Name                 | Type                                 | Description                                |
+| -------------------- | ------------------------------------ | ------------------------------------------ |
+| `~input/vector_map`  | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2                     |
+| `~input/goal_pose`   | geometry_msgs/PoseStamped            | goal pose                                  |
+| `~input/checkpoints` | geometry_msgs/PoseStamped            | checkpoint to follow while heading to goal |
+
+### output
+
+| Name            | Type                                    | Description                 |
+| --------------- | --------------------------------------- | --------------------------- |
+| `~output/route` | autoware_auto_planning_msgs/HADMapRoute | route from ego pose to goal |
+
+`autoware_planning_msgs/Route` consists of route sections and goal pose.
+
+![route_sections](./media/route_sections.svg)
+
+Route section, whose type is `autoware_auto_mapping_msgs/HADMapSegment`, is a "slice" of a road that bundles lane changeable lanes.
+Note that the most atomic unit of route is `autoware_auto_mapping_msgs/MapPrimitive`, which has the unique id of a lane in a vector map and its type.
+Therefore, route message does not contain geometric information about the lane since we did not want to have planning module’s message to have dependency on map data structure.
+
+The ROS message of route section contains following three elements for each route section.
+
+- `preferred_primitive_id`: Preferred lane to follow towards the goal.
+- `primitives`: All neighbor lanes in the same direction including the preferred lane.
+
+## Implementation
+
+### Mission Planner
+
+Two callbacks (goal and check points) are a trigger for route planning.
+Routing graph, which plans route in Lanelet2, must be created before those callbacks, and this routing graph is created in vector map callback.
+
+`plan route` is explained in detail in the following section.
+
+```plantuml
+@startuml
+title goal callback
+start
+
+:clear previously memorized check points;
+
+:memorize ego and goal pose as check points;
+
+if (routing graph is ready?) then (yes)
+else (no)
+  stop
+endif
+
+:plan route;
+
+:publish route;
+
+stop
+@enduml
+```
+
+Note that during the goal callback, previously memorized check points are removed, and only current ego pose and goal pose are memorized as check points.
+
+```plantuml
+@startuml
+title check point callback
+start
+
+if (size of check points >= 2?) then (yes)
+else (no)
+  stop
+endif
+
+:memorize check point;
+
+:plan route;
+
+:publish route;
+
+stop
+@enduml
+```
+
+Note that at least two check points must be already memorized, which are start and goal pose, before the check point callback.
+
+### Route Planner
+
+`plan route` is executed with check points including current ego pose and goal pose.
+
+```plantuml
+@startuml
+title plan route
+start
+
+if (goal is valid?) then (yes)
+else (no)
+  stop
+endif
+
+:plan path between each check points;
+
+:initialize route lanelets;
+
+:get preferred lanelets;
+
+:create route sections;
+
+if (planed route is looped?) then (no)
+else (yes)
+  :warn that looped route is not supported;
+endif
+
+:return route;
+
+stop
+@enduml
+```
+
+`plan path between each check points` firstly calculates closest lanes to start and goal pose.
+Then routing graph of Lanelet2 plans the shortest path from start and goal pose.
+
+`initialize route lanelets` initializes route handler, and calculates `route_lanelets`.
+`route_lanelets`, all of which will be registered in route sections, are lanelets next to the lanelets in the planned path, and used when planning lane change.
+To calculate `route_lanelets`,
+
+1. All the neighbor (right and left) lanes for the planned path which is lane-changeable is memorized as `route_lanelets`.
+2. All the neighbor (right and left) lanes for the planned path which is not lane-changeable is memorized as `candidate_lanelets`.
+3. If the following and previous lanelets of each `candidate_lanelets` are `route_lanelets`, the `candidate_lanelet` is registered as `route_lanelets`
+   - This is because even though `candidate_lanelet` (an adjacent lane) is not lane-changeable, we can pass the `candidate_lanelet` without lane change if the following and previous lanelets of the `candidate_lanelet` are `route_lanelets`
+
+`get preferred lanelets` extracts `preferred_primitive_id` from `route_lanelets` with the route handler.
+
+`create route sections` extracts `primitives` from `route_lanelets` for each route section with the route handler, and creates route sections.
+
+## Limitations
+
+- Dynamic objects (e.g. pedestrians and other vehicles) and dynamic map information (e.g. road construction which blocks some lanes) are not considered during route planning.
+- Looped route is not supported.
diff --git a/planning/mission_planner/include/mission_planner/goal_pose_visualizer.hpp b/planning/mission_planner/include/mission_planner/goal_pose_visualizer.hpp
@@ -0,0 +1,39 @@
+// Copyright 2020 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_
+#define MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+namespace mission_planner
+{
+class GoalPoseVisualizer : public rclcpp::Node
+{
+public:
+  explicit GoalPoseVisualizer(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr sub_route_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_goal_pose_;
+
+  void echoBackRouteCallback(
+    const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg);
+};
+
+}  // namespace mission_planner
+#endif  // MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_
diff --git a/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp b/planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp
@@ -0,0 +1,69 @@
+// Copyright 2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MISSION_PLANNER__LANELET2_IMPL__MISSION_PLANNER_LANELET2_HPP_
+#define MISSION_PLANNER__LANELET2_IMPL__MISSION_PLANNER_LANELET2_HPP_
+
+#include <string>
+#include <vector>
+
+// ROS
+#include <rclcpp/rclcpp.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+// Autoware
+#include "mission_planner/mission_planner_base.hpp"
+
+#include <route_handler/route_handler.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+
+// lanelet
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+namespace mission_planner
+{
+using RouteSections = std::vector<autoware_auto_mapping_msgs::msg::HADMapSegment>;
+class MissionPlannerLanelet2 : public MissionPlanner
+{
+public:
+  explicit MissionPlannerLanelet2(const rclcpp::NodeOptions & node_options);
+
+private:
+  bool is_graph_ready_;
+  lanelet::LaneletMapPtr lanelet_map_ptr_;
+  lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
+  lanelet::ConstLanelets road_lanelets_;
+  lanelet::ConstLanelets shoulder_lanelets_;
+  route_handler::RouteHandler route_handler_;
+
+  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_subscriber_;
+
+  void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
+  bool isGoalValid() const;
+
+  // virtual functions
+  bool isRoutingGraphReady() const;
+  autoware_auto_planning_msgs::msg::HADMapRoute planRoute();
+  void visualizeRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const;
+};
+}  // namespace mission_planner
+
+#endif  // MISSION_PLANNER__LANELET2_IMPL__MISSION_PLANNER_LANELET2_HPP_
diff --git a/planning/mission_planner/include/mission_planner/lanelet2_impl/utility_functions.hpp b/planning/mission_planner/include/mission_planner/lanelet2_impl/utility_functions.hpp
@@ -0,0 +1,46 @@
+// Copyright 2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT 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 MISSION_PLANNER__LANELET2_IMPL__UTILITY_FUNCTIONS_HPP_
+#define MISSION_PLANNER__LANELET2_IMPL__UTILITY_FUNCTIONS_HPP_
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/primitives/LaneletSequence.h>
+
+#include <string>
+#include <unordered_set>
+#include <vector>
+bool exists(const std::unordered_set<lanelet::Id> & set, const lanelet::Id & id);
+
+template <typename T>
+bool exists(const std::vector<T> & vectors, const T & item)
+{
+  for (const auto & i : vectors) {
+    if (i == item) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a);
+void insertMarkerArray(
+  visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);
+std::string toString(const geometry_msgs::msg::Pose & pose);
+#endif  // MISSION_PLANNER__LANELET2_IMPL__UTILITY_FUNCTIONS_HPP_