From ebe771fbb15280f5a6030177fe4e73f38fc7a08a Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 7 Dec 2021 10:03:06 +0900 Subject: [PATCH] feat: add mission_planner package (#40) * release v0.4.0 * Fix routing from crosswalk (#767) Signed-off-by: Daisuke Nishimatsu * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit 5eba353a7573c7e12bc80cd814c95113cb20c077. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * port mission_planner to ROS2 (#56) * port mission_planner to ROS2 Signed-off-by: mitsudome-r * fix QoS for publishers Signed-off-by: mitsudome-r * Add geometry2 to repos (#76) * add geometry2 package temporarily until new release Signed-off-by: mitsudome-r * trigger-ci Signed-off-by: mitsudome-r * add tf2 dependency to the packages that use tf2_geometry_msgs Signed-off-by: mitsudome-r * Revert "Add geometry2 to repos (#76)" (#96) * Revert "Add geometry2 to repos (#76)" This reverts commit 61defd01accd71fa65837e6317f8c6b693a32254. * 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 * Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * fixing trasient_local in ROS2 packages (#160) * Add linters to mission_planner (#156) * Added linters to mission_planner * Removed duplicate dependencies * Only add the cppcheck linter * Added linters to CMake * Ros2 v0.8.0 mission planner (#278) * add use_sim-time option (#454) * Fix rolling build errors (#1225) * Add missing include files Signed-off-by: Kenji Miyake * Replace rclcpp::Duration Signed-off-by: Kenji Miyake * Use reference for exceptions Signed-off-by: Kenji Miyake * Use from_seconds Signed-off-by: Kenji Miyake * Unify Apache-2.0 license name (#1242) * Make planning modules components (#1263) Signed-off-by: wep21 * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Fix compiler warnings (#1837) * Fix -Wunused-private-field Signed-off-by: Kenji Miyake * Fix -Wunused-variable Signed-off-by: Kenji Miyake * Fix -Wformat-security Signed-off-by: Kenji Miyake * Fix -Winvalid-constexpr Signed-off-by: Kenji Miyake * Fix -Wdelete-non-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake * Fix -Wdelete-abstract-non-virtual-dtor Signed-off-by: Kenji Miyake * Fix -Winconsistent-missing-override Signed-off-by: Kenji Miyake * Fix -Wrange-loop-construct Signed-off-by: Kenji Miyake * Fix "invalid application of 'sizeof' to an incomplete type" Signed-off-by: Kenji Miyake * Ignore -Wgnu-anonymous-struct and -Wnested-anon-types Signed-off-by: Kenji Miyake * Fix lint Signed-off-by: Kenji Miyake * Ignore -Wno-deprecated-declarations in CUDA-related packages Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Docs/mission planner (#1952) * add doc * update docs * Invoke code formatter at pre-commit (#1935) * Run ament_uncrustify at pre-commit * Reformat existing files * Fix copyright and cpplint errors Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake * update mission planner doc (#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 (#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 (#2147) * Change formatter to clang-format and black (#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 * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * rename to README.md (#550) * rename to README.md * dealt with new auto_msgs format Co-authored-by: Takayuki Murooka * port mission planner (#538) * port lanelet2 msg Signed-off-by: Kosuke Murakami * port route Signed-off-by: Kosuke Murakami * fix precommit Signed-off-by: Kosuke Murakami * Use route_handler package in mission_planner (#579) * [autoware_vehicle_rviz_plugin/route_handler/simple_planning_simulator]fix some packages (#606) * fix console meter * fix velocity_history * fix route handler * change topic name * Modify readme for mission planner (#714) * modify readme for mission planner Signed-off-by: kosuke murakami * fix document Signed-off-by: kosuke murakami * fix readme Co-authored-by: mitsudome-r Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Nikolai Morin Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Esteve Fernandez Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Takamasa Horibe Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Hiroki OTA Co-authored-by: Takayuki Murooka Co-authored-by: Takeshi Ishita Co-authored-by: Kenji Miyake 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 Co-authored-by: Kosuke Murakami Co-authored-by: Fumiya Watanabe Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> --- planning/mission_planner/CMakeLists.txt | 44 +++ planning/mission_planner/README.md | 149 +++++++++ .../mission_planner/goal_pose_visualizer.hpp | 39 +++ .../mission_planner_lanelet2.hpp | 69 ++++ .../lanelet2_impl/utility_functions.hpp | 46 +++ .../mission_planner/mission_planner_base.hpp | 73 +++++ .../launch/goal_pose_visualizer.launch.xml | 9 + .../launch/mission_planner.launch.xml | 17 + .../lib/mission_planner_base.cpp | 147 +++++++++ .../mission_planner/media/route_sections.svg | 3 + planning/mission_planner/package.xml | 28 ++ .../goal_pose_visualizer.cpp | 40 +++ .../mission_planner_lanelet2.cpp | 300 ++++++++++++++++++ .../utility_functions.cpp | 67 ++++ 14 files changed, 1031 insertions(+) create mode 100644 planning/mission_planner/CMakeLists.txt create mode 100644 planning/mission_planner/README.md create mode 100644 planning/mission_planner/include/mission_planner/goal_pose_visualizer.hpp create mode 100644 planning/mission_planner/include/mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp create mode 100644 planning/mission_planner/include/mission_planner/lanelet2_impl/utility_functions.hpp create mode 100644 planning/mission_planner/include/mission_planner/mission_planner_base.hpp create mode 100644 planning/mission_planner/launch/goal_pose_visualizer.launch.xml create mode 100644 planning/mission_planner/launch/mission_planner.launch.xml create mode 100644 planning/mission_planner/lib/mission_planner_base.cpp create mode 100644 planning/mission_planner/media/route_sections.svg create mode 100644 planning/mission_planner/package.xml create mode 100644 planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp create mode 100644 planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp create mode 100644 planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt new file mode 100644 index 0000000000000..b7a9b3ce9c84f --- /dev/null +++ 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 new file mode 100644 index 0000000000000..b158ff4a00894 --- /dev/null +++ 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 new file mode 100644 index 0000000000000..65b3f91e4a9f0 --- /dev/null +++ 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 + +#include +#include + +namespace mission_planner +{ +class GoalPoseVisualizer : public rclcpp::Node +{ +public: + explicit GoalPoseVisualizer(const rclcpp::NodeOptions & node_options); + +private: + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Publisher::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 new file mode 100644 index 0000000000000..a623f45e432ba --- /dev/null +++ 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 +#include + +// ROS +#include + +#include +#include + +// Autoware +#include "mission_planner/mission_planner_base.hpp" + +#include + +#include +#include + +// lanelet +#include +#include +#include + +namespace mission_planner +{ +using RouteSections = std::vector; +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::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 new file mode 100644 index 0000000000000..e4616aaaae284 --- /dev/null +++ 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 + +#include +#include +#include + +#include +#include + +#include +#include +#include +bool exists(const std::unordered_set & set, const lanelet::Id & id); + +template +bool exists(const std::vector & 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_ diff --git a/planning/mission_planner/include/mission_planner/mission_planner_base.hpp b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp new file mode 100644 index 0000000000000..c7c56479f3f2f --- /dev/null +++ b/planning/mission_planner/include/mission_planner/mission_planner_base.hpp @@ -0,0 +1,73 @@ +// 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__MISSION_PLANNER_BASE_HPP_ +#define MISSION_PLANNER__MISSION_PLANNER_BASE_HPP_ + +#include +#include + +// ROS +#include + +#include + +#include +#include + +// Autoware +#include +#include +#include + +namespace mission_planner +{ +class MissionPlanner : public rclcpp::Node +{ +protected: + MissionPlanner(const std::string & node_name, const rclcpp::NodeOptions & node_options); + + geometry_msgs::msg::PoseStamped goal_pose_; + geometry_msgs::msg::PoseStamped start_pose_; + std::vector checkpoints_; + + std::string base_link_frame_; + std::string map_frame_; + + rclcpp::Publisher::SharedPtr marker_publisher_; + + virtual bool isRoutingGraphReady() const = 0; + virtual autoware_auto_planning_msgs::msg::HADMapRoute planRoute() = 0; + virtual void visualizeRoute( + const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0; + virtual void publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const; + +private: + rclcpp::Publisher::SharedPtr route_publisher_; + rclcpp::Subscription::SharedPtr goal_subscriber_; + rclcpp::Subscription::SharedPtr checkpoint_subscriber_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + bool getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose); + void goalPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr); + void checkpointCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr); + bool transformPose( + const geometry_msgs::msg::PoseStamped & input_pose, + geometry_msgs::msg::PoseStamped * output_pose, const std::string target_frame); +}; + +} // namespace mission_planner +#endif // MISSION_PLANNER__MISSION_PLANNER_BASE_HPP_ diff --git a/planning/mission_planner/launch/goal_pose_visualizer.launch.xml b/planning/mission_planner/launch/goal_pose_visualizer.launch.xml new file mode 100644 index 0000000000000..69f1724675c15 --- /dev/null +++ b/planning/mission_planner/launch/goal_pose_visualizer.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/planning/mission_planner/launch/mission_planner.launch.xml b/planning/mission_planner/launch/mission_planner.launch.xml new file mode 100644 index 0000000000000..70b2893eca153 --- /dev/null +++ b/planning/mission_planner/launch/mission_planner.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/planning/mission_planner/lib/mission_planner_base.cpp b/planning/mission_planner/lib/mission_planner_base.cpp new file mode 100644 index 0000000000000..2d29b655602a6 --- /dev/null +++ b/planning/mission_planner/lib/mission_planner_base.cpp @@ -0,0 +1,147 @@ +// 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. + +#include "mission_planner/mission_planner_base.hpp" + +#include +#include +#include + +#include +#include +#include + +#include + +namespace mission_planner +{ +MissionPlanner::MissionPlanner( + const std::string & node_name, const rclcpp::NodeOptions & node_options) +: Node(node_name, node_options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) +{ + map_frame_ = declare_parameter("map_frame", "map"); + base_link_frame_ = declare_parameter("base_link_frame", "base_link"); + + using std::placeholders::_1; + + goal_subscriber_ = create_subscription( + "input/goal_pose", 10, std::bind(&MissionPlanner::goalPoseCallback, this, _1)); + checkpoint_subscriber_ = create_subscription( + "input/checkpoint", 10, std::bind(&MissionPlanner::checkpointCallback, this, _1)); + + rclcpp::QoS durable_qos{1}; + durable_qos.transient_local(); + route_publisher_ = + create_publisher("output/route", durable_qos); + marker_publisher_ = + create_publisher("debug/route_marker", durable_qos); +} + +bool MissionPlanner::getEgoVehiclePose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose) +{ + geometry_msgs::msg::PoseStamped base_link_origin; + base_link_origin.header.frame_id = base_link_frame_; + base_link_origin.pose.position.x = 0; + base_link_origin.pose.position.y = 0; + base_link_origin.pose.position.z = 0; + base_link_origin.pose.orientation.x = 0; + base_link_origin.pose.orientation.y = 0; + base_link_origin.pose.orientation.z = 0; + base_link_origin.pose.orientation.w = 1; + + // transform base_link frame origin to map_frame to get vehicle positions + return transformPose(base_link_origin, ego_vehicle_pose, map_frame_); +} + +bool MissionPlanner::transformPose( + const geometry_msgs::msg::PoseStamped & input_pose, geometry_msgs::msg::PoseStamped * output_pose, + const std::string target_frame) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = + tf_buffer_.lookupTransform(target_frame, input_pose.header.frame_id, tf2::TimePointZero); + tf2::doTransform(input_pose, *output_pose, transform); + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + return false; + } +} + +void MissionPlanner::goalPoseCallback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr) +{ + // set start pose + if (!getEgoVehiclePose(&start_pose_)) { + RCLCPP_ERROR( + get_logger(), "Failed to get ego vehicle pose in map frame. Aborting mission planning"); + return; + } + // set goal pose + if (!transformPose(*goal_msg_ptr, &goal_pose_, map_frame_)) { + RCLCPP_ERROR(get_logger(), "Failed to get goal pose in map frame. Aborting mission planning"); + return; + } + + RCLCPP_INFO(get_logger(), "New goal pose is set. Reset checkpoints."); + checkpoints_.clear(); + checkpoints_.push_back(start_pose_); + checkpoints_.push_back(goal_pose_); + + if (!isRoutingGraphReady()) { + RCLCPP_ERROR(get_logger(), "RoutingGraph is not ready. Aborting mission planning"); + return; + } + + autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(); + publishRoute(route); +} // namespace mission_planner + +void MissionPlanner::checkpointCallback( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr) +{ + if (checkpoints_.size() < 2) { + RCLCPP_ERROR( + get_logger(), + "You must set start and goal before setting checkpoints. Aborting mission planning"); + return; + } + + geometry_msgs::msg::PoseStamped transformed_checkpoint; + if (!transformPose(*checkpoint_msg_ptr, &transformed_checkpoint, map_frame_)) { + RCLCPP_ERROR( + get_logger(), "Failed to get checkpoint pose in map frame. Aborting mission planning"); + return; + } + + // insert checkpoint before goal + checkpoints_.insert(checkpoints_.end() - 1, transformed_checkpoint); + + autoware_auto_planning_msgs::msg::HADMapRoute route = planRoute(); + publishRoute(route); +} + +void MissionPlanner::publishRoute(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const +{ + if (!route.segments.empty()) { + RCLCPP_INFO(get_logger(), "Route successfully planned. Publishing..."); + route_publisher_->publish(route); + visualizeRoute(route); + } else { + RCLCPP_ERROR(get_logger(), "Calculated route is empty!"); + } +} + +} // namespace mission_planner diff --git a/planning/mission_planner/media/route_sections.svg b/planning/mission_planner/media/route_sections.svg new file mode 100644 index 0000000000000..39db8562d696c --- /dev/null +++ b/planning/mission_planner/media/route_sections.svg @@ -0,0 +1,3 @@ + + +
Route Section 4
Route Sectio...
Route Section 3
Route Sectio...
Route Section 2
Route Sectio...
Route Section 1
Route Sectio...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml new file mode 100644 index 0000000000000..3aced14bbb08a --- /dev/null +++ b/planning/mission_planner/package.xml @@ -0,0 +1,28 @@ + + + mission_planner + 0.1.0 + The mission_planner package + + mitsudome-r + + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_planning_msgs + geometry_msgs + lanelet2_extension + rclcpp + rclcpp_components + route_handler + tf2_geometry_msgs + tf2_ros + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp b/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp new file mode 100644 index 0000000000000..8f9e466fa78ff --- /dev/null +++ b/planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp @@ -0,0 +1,40 @@ +// 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. + +#include "mission_planner/goal_pose_visualizer.hpp" + +namespace mission_planner +{ +GoalPoseVisualizer::GoalPoseVisualizer(const rclcpp::NodeOptions & node_options) +: Node("goal_pose_visualizer_node", node_options) +{ + sub_route_ = create_subscription( + "input/route", rclcpp::QoS{1}, + std::bind(&GoalPoseVisualizer::echoBackRouteCallback, this, std::placeholders::_1)); + pub_goal_pose_ = create_publisher( + "output/goal_pose", rclcpp::QoS{1}.transient_local()); +} + +void GoalPoseVisualizer::echoBackRouteCallback( + const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg) +{ + geometry_msgs::msg::PoseStamped goal_pose; + goal_pose.header = msg->header; + goal_pose.pose = msg->goal_pose; + pub_goal_pose_->publish(goal_pose); +} +} // namespace mission_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mission_planner::GoalPoseVisualizer) diff --git a/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp new file mode 100644 index 0000000000000..271b4a0344115 --- /dev/null +++ b/planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.cpp @@ -0,0 +1,300 @@ +// 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. + +#include "mission_planner/lanelet2_impl/mission_planner_lanelet2.hpp" + +#include "mission_planner/lanelet2_impl/utility_functions.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ +using RouteSections = std::vector; +RouteSections combineConsecutiveRouteSections( + const RouteSections & route_sections1, const RouteSections & route_sections2) +{ + RouteSections route_sections; + route_sections.reserve(route_sections1.size() + route_sections2.size()); + if (!route_sections1.empty()) { + // remove end route section because it is overlapping with first one in next route_section + route_sections.insert(route_sections.end(), route_sections1.begin(), route_sections1.end() - 1); + } + if (!route_sections2.empty()) { + route_sections.insert(route_sections.end(), route_sections2.begin(), route_sections2.end()); + } + return route_sections; +} + +bool isRouteLooped(const RouteSections & route_sections) +{ + for (std::size_t i = 0; i < route_sections.size(); i++) { + const auto & route_section = route_sections.at(i); + for (const auto & lane_id : route_section.primitives) { + for (std::size_t j = i + 1; j < route_sections.size(); j++) { + const auto & future_route_section = route_sections.at(j); + if (exists(future_route_section.primitives, lane_id)) { + return true; + } + } + } + } + return false; +} + +double normalizeRadian(const double rad, const double min_rad = -M_PI, const double max_rad = M_PI) +{ + const auto value = std::fmod(rad, 2 * M_PI); + if (min_rad < value && value <= max_rad) { + return value; + } else { + return value - std::copysign(2 * M_PI, value); + } +} + +bool isInLane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) +{ + // check if goal is on a lane at appropriate angle + const auto distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + return distance < th_distance; +} + +bool isInParkingSpace( + const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_space : parking_spaces) { + lanelet::ConstPolygon3d parking_space_polygon; + if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) { + continue; + } + + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_space_polygon).basicPolygon(), + lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +bool isInParkingLot( + const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_lot : parking_lots) { + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +} // anonymous namespace + +namespace mission_planner +{ +MissionPlannerLanelet2::MissionPlannerLanelet2(const rclcpp::NodeOptions & node_options) +: MissionPlanner("mission_planner_node", node_options), is_graph_ready_(false) +{ + using std::placeholders::_1; + map_subscriber_ = create_subscription( + "input/vector_map", rclcpp::QoS{10}.transient_local(), + std::bind(&MissionPlannerLanelet2::mapCallback, this, _1)); +} + +void MissionPlannerLanelet2::mapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + route_handler_.setMap(*msg); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); + is_graph_ready_ = true; +} + +bool MissionPlannerLanelet2::isRoutingGraphReady() const { return is_graph_ready_; } + +void MissionPlannerLanelet2::visualizeRoute( + const autoware_auto_planning_msgs::msg::HADMapRoute & route) const +{ + lanelet::ConstLanelets route_lanelets; + lanelet::ConstLanelets end_lanelets; + lanelet::ConstLanelets normal_lanelets; + lanelet::ConstLanelets goal_lanelets; + + for (const auto & route_section : route.segments) { + for (const auto & lane_id : route_section.primitives) { + auto lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id.id); + route_lanelets.push_back(lanelet); + if (route_section.preferred_primitive_id == lane_id.id) { + goal_lanelets.push_back(lanelet); + } else { + end_lanelets.push_back(lanelet); + } + } + } + + std_msgs::msg::ColorRGBA cl_route, cl_ll_borders, cl_end, cl_normal, cl_goal; + setColor(&cl_route, 0.2, 0.4, 0.2, 0.05); + setColor(&cl_goal, 0.2, 0.4, 0.4, 0.05); + setColor(&cl_end, 0.2, 0.2, 0.4, 0.05); + setColor(&cl_normal, 0.2, 0.4, 0.2, 0.05); + setColor(&cl_ll_borders, 1.0, 1.0, 1.0, 0.999); + + visualization_msgs::msg::MarkerArray route_marker_array; + insertMarkerArray( + &route_marker_array, + lanelet::visualization::laneletsBoundaryAsMarkerArray(route_lanelets, cl_ll_borders, false)); + insertMarkerArray( + &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "route_lanelets", route_lanelets, cl_route)); + insertMarkerArray( + &route_marker_array, + lanelet::visualization::laneletsAsTriangleMarkerArray("end_lanelets", end_lanelets, cl_end)); + insertMarkerArray( + &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( + "normal_lanelets", normal_lanelets, cl_normal)); + insertMarkerArray( + &route_marker_array, + lanelet::visualization::laneletsAsTriangleMarkerArray("goal_lanelets", goal_lanelets, cl_goal)); + marker_publisher_->publish(route_marker_array); +} + +bool MissionPlannerLanelet2::isGoalValid() const +{ + lanelet::Lanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLanelet( + road_lanelets_, goal_pose_.pose, &closest_lanelet)) { + return false; + } + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal_pose_.pose.position); + + if (isInLane(closest_lanelet, goal_lanelet_pt)) { + const auto lane_yaw = + lanelet::utils::getLaneletAngle(closest_lanelet, goal_pose_.pose.position); + const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); + const auto angle_diff = normalizeRadian(lane_yaw - goal_yaw); + + constexpr double th_angle = M_PI / 4; + + if (std::abs(angle_diff) < th_angle) { + return true; + } + } + + // check if goal is in parking space + const auto parking_spaces = lanelet::utils::query::getAllParkingSpaces(lanelet_map_ptr_); + if (isInParkingSpace(parking_spaces, goal_lanelet_pt)) { + return true; + } + + // check if goal is in parking lot + const auto parking_lots = lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_); + if (isInParkingLot(parking_lots, goal_lanelet_pt)) { + return true; + } + + // check if goal is in shoulder lanelet + lanelet::Lanelet closest_shoulder_lanelet; + if (!lanelet::utils::query::getClosestLanelet( + shoulder_lanelets_, goal_pose_.pose, &closest_shoulder_lanelet)) { + return false; + } + // check if goal pose is in shoulder lane + if (isInLane(closest_shoulder_lanelet, goal_lanelet_pt)) { + const auto lane_yaw = + lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal_pose_.pose.position); + const auto goal_yaw = tf2::getYaw(goal_pose_.pose.orientation); + const auto angle_diff = normalizeRadian(lane_yaw - goal_yaw); + + constexpr double th_angle = M_PI / 4; + if (std::abs(angle_diff) < th_angle) { + return true; + } + } + + return false; +} + +autoware_auto_planning_msgs::msg::HADMapRoute MissionPlannerLanelet2::planRoute() +{ + std::stringstream ss; + for (const auto & checkpoint : checkpoints_) { + ss << "x: " << checkpoint.pose.position.x << " " + << "y: " << checkpoint.pose.position.y << std::endl; + } + RCLCPP_INFO_STREAM( + get_logger(), "start planning route with checkpoints: " << std::endl + << ss.str()); + + autoware_auto_planning_msgs::msg::HADMapRoute route_msg; + RouteSections route_sections; + + if (!isGoalValid()) { + RCLCPP_WARN(get_logger(), "Goal is not valid! Please check position and angle of goal_pose"); + return route_msg; + } + + for (std::size_t i = 1; i < checkpoints_.size(); i++) { + const auto start_checkpoint = checkpoints_.at(i - 1); + const auto goal_checkpoint = checkpoints_.at(i); + lanelet::ConstLanelets path_lanelets; + if (!route_handler_.planPathLaneletsBetweenCheckpoints( + start_checkpoint.pose, goal_checkpoint.pose, &path_lanelets)) { + return route_msg; + } + // create local route sections + route_handler_.setRouteLanelets(path_lanelets); + const auto local_route_sections = route_handler_.createMapSegments(path_lanelets); + route_sections = combineConsecutiveRouteSections(route_sections, local_route_sections); + } + + if (isRouteLooped(route_sections)) { + RCLCPP_WARN( + get_logger(), "Loop detected within route! Be aware that looped route is not debugged!"); + } + + route_msg.header.stamp = this->now(); + route_msg.header.frame_id = map_frame_; + route_msg.segments = route_sections; + route_msg.goal_pose = goal_pose_.pose; + + return route_msg; +} + +} // namespace mission_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mission_planner::MissionPlannerLanelet2) diff --git a/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp new file mode 100644 index 0000000000000..a4f298e6890e7 --- /dev/null +++ b/planning/mission_planner/src/mission_planner_lanelet2/utility_functions.cpp @@ -0,0 +1,67 @@ +// 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. + +#include "mission_planner/lanelet2_impl/utility_functions.hpp" + +#include + +#include + +#include +#include +#include +#include + +bool exists(const std::unordered_set & set, const lanelet::Id & id) +{ + return set.find(id) != set.end(); +} + +std::string toString(const geometry_msgs::msg::Pose & pose) +{ + std::stringstream ss; + ss << "(" << pose.position.x << ", " << pose.position.y << "," << pose.position.z << ")"; + return ss.str(); +} + +void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) +{ + cl->r = r; + cl->g = g; + cl->b = b; + cl->a = a; +} + +void insertMarkerArray( + visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) +{ + a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); +} + +std::vector> excludeSubtypeLaneletsWithDistance( + const std::vector> & lls, const char subtype[]) +{ + std::vector> exclude_subtype_lanelets; + + for (const auto & ll : lls) { + if (ll.second.hasAttribute(lanelet::AttributeName::Subtype)) { + lanelet::Attribute attr = ll.second.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != subtype) { + exclude_subtype_lanelets.push_back(ll); + } + } + } + + return exclude_subtype_lanelets; +}