feat: add route_handler package (#24)

* Feature/route handler (#572)

* Add route_handler library

* Add explicit in constructor

* Fix depend

* [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

* Port behavior path planner (#622)

* Convert message to autoware_auto_msgs

* Use route_handler package and fix bugs

* Fix README

* Fix hazard signal

* Fix default value of TurnSignalInfo and use get_clock()

* Use odometry

* clear route_handler when initializing route_handler for mission_planning (#747)

* add readme

* ci(pre-commit): autofix

Co-authored-by: Fumiya Watanabe <[email protected]>
Co-authored-by: Takayuki Murooka <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/route_handler/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.5)
+project(route_handler)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(route_handler SHARED
+  src/route_handler.cpp
+)
+
+# Test
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

planning/route_handler/README.md

@@ -0,0 +1,3 @@
+# route handler
+
+`route_handler` is a library for calculating driving route on the lanelet map.

planning/route_handler/include/route_handler/route_handler.hpp

@@ -0,0 +1,211 @@
+// Copyright 2021 Tier IV, Inc.
+//
+// Licensed under the Apache 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 ROUTE_HANDLER__ROUTE_HANDLER_HPP_
+#define ROUTE_HANDLER__ROUTE_HANDLER_HPP_
+
+#include <autoware_utils/autoware_utils.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_mapping_msgs/msg/had_map_segment.hpp>
+#include <autoware_auto_mapping_msgs/msg/map_primitive.hpp>
+#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
+#include <autoware_auto_planning_msgs/msg/path.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <lanelet2_routing/Route.h>
+#include <lanelet2_routing/RoutingCost.h>
+#include <lanelet2_routing/RoutingGraph.h>
+#include <lanelet2_routing/RoutingGraphContainer.h>
+#include <lanelet2_traffic_rules/TrafficRulesFactory.h>
+
+#include <limits>
+#include <memory>
+#include <vector>
+
+namespace route_handler
+{
+using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_mapping_msgs::msg::HADMapSegment;
+using autoware_auto_planning_msgs::msg::HADMapRoute;
+using autoware_auto_planning_msgs::msg::Path;
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseStamped;
+using std_msgs::msg::Header;
+
+enum class LaneChangeDirection { NONE, LEFT, RIGHT };
+enum class PullOverDirection { NONE, LEFT, RIGHT };
+enum class PullOutDirection { NONE, LEFT, RIGHT };
+
+class RouteHandler
+{
+public:
+  RouteHandler() = default;
+  explicit RouteHandler(const HADMapBin & map_msg);
+
+  // non-const methods
+  void setMap(const HADMapBin & map_msg);
+  void setRoute(const HADMapRoute & route_msg);
+  void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets);
+  void setPullOverGoalPose(
+    const lanelet::ConstLanelet target_lane, const double vehicle_width, const double margin);
+
+  // const methods
+
+  // for route handler status
+  bool isHandlerReady() const;
+  lanelet::ConstPolygon3d getExtraDrivableAreaById(const lanelet::Id id) const;
+  Header getRouteHeader() const;
+  lanelet::routing::RoutingGraphContainer getOverallGraph() const;
+
+  // for routing
+  bool planPathLaneletsBetweenCheckpoints(
+    const Pose & start_checkpoint, const Pose & goal_checkpoint,
+    lanelet::ConstLanelets * path_lanelets) const;
+  std::vector<HADMapSegment> createMapSegments(const lanelet::ConstLanelets & path_lanelets) const;
+
+  // for goal
+  bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const;
+  Pose getGoalPose() const;
+  Pose getPullOverGoalPose() const;
+  lanelet::Id getGoalLaneId() const;
+  bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const;
+  std::vector<lanelet::ConstLanelet> getLanesAfterGoal(const double vehicle_length) const;
+
+  // for lanelet
+  bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const;
+  int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const;
+  bool getClosestLaneletWithinRoute(
+    const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const;
+  lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const;
+  lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids ids) const;
+  lanelet::ConstLanelets getLaneletSequence(
+    const lanelet::ConstLanelet & lanelet, const Pose & current_pose,
+    const double backward_distance, const double forward_distance) const;
+  lanelet::ConstLanelets getLaneletSequence(
+    const lanelet::ConstLanelet & lanelet,
+    const double backward_distance = std::numeric_limits<double>::max(),
+    const double forward_distance = std::numeric_limits<double>::max()) const;
+  lanelet::ConstLanelets getShoulderLaneletSequence(
+    const lanelet::ConstLanelet & lanelet, const Pose & current_pose,
+    const double backward_distance = std::numeric_limits<double>::max(),
+    const double forward_distance = std::numeric_limits<double>::max()) const;
+  lanelet::ConstLanelets getCheckTargetLanesFromPath(
+    const PathWithLaneId & path, const lanelet::ConstLanelets & target_lanes,
+    const double check_length) const;
+  lanelet::routing::RelationType getRelation(
+    const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const;
+  lanelet::ConstLanelets getShoulderLanelets() const;
+
+  // for path
+  PathWithLaneId getCenterLinePath(
+    const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end,
+    bool use_exact = true) const;
+  bool getLaneChangeTarget(
+    const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const;
+  bool getPullOverTarget(
+    const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const;
+  bool getPullOutStart(
+    const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet,
+    const Pose & pose, const double vehicle_width) const;
+  double getLaneChangeableDistance(
+    const Pose & current_pose, const LaneChangeDirection & direction) const;
+  lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
+
+private:
+  // MUST
+  lanelet::routing::RoutingGraphPtr routing_graph_ptr_;
+  lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_;
+  std::shared_ptr<const lanelet::routing::RoutingGraphContainer> overall_graphs_ptr_;
+  lanelet::LaneletMapPtr lanelet_map_ptr_;
+  lanelet::ConstLanelets road_lanelets_;
+  lanelet::ConstLanelets route_lanelets_;
+  lanelet::ConstLanelets preferred_lanelets_;
+  lanelet::ConstLanelets start_lanelets_;
+  lanelet::ConstLanelets goal_lanelets_;
+  lanelet::ConstLanelets shoulder_lanelets_;
+  Pose pull_over_goal_pose_;
+  HADMapRoute route_msg_;
+
+  rclcpp::Logger logger_{rclcpp::get_logger("route_handler")};
+
+  bool is_route_msg_ready_{false};
+  bool is_map_msg_ready_{false};
+  bool is_handler_ready_{false};
+
+  // non-const methods
+  void setLaneletsFromRouteMsg();
+
+  // const methods
+  // for routing
+  lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const;
+
+  // for lanelet
+  bool isInTargetLane(const PoseStamped & pose, const lanelet::ConstLanelets & target) const;
+  bool isInPreferredLane(const PoseStamped & pose) const;
+  bool isBijectiveConnection(
+    const lanelet::ConstLanelets & lanelet_section1,
+    const lanelet::ConstLanelets & lanelet_section2) const;
+  bool getPreviousLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
+  bool getNextLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
+  bool getPreviousLaneletWithinRouteExceptGoal(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
+  bool getNextLaneletWithinRouteExceptStart(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const;
+  bool getRightLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
+  bool getLeftLaneletWithinRoute(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const;
+  lanelet::ConstLanelets getRouteLanelets() const;
+  lanelet::ConstLanelets getLaneletSequenceUpTo(
+    const lanelet::ConstLanelet & lanelet,
+    const double min_length = std::numeric_limits<double>::max()) const;
+  lanelet::ConstLanelets getLaneletSequenceAfter(
+    const lanelet::ConstLanelet & lanelet,
+    const double min_length = std::numeric_limits<double>::max()) const;
+  bool getFollowingShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * following_lanelet) const;
+  lanelet::ConstLanelets getShoulderLaneletSequenceAfter(
+    const lanelet::ConstLanelet & lanelet,
+    const double min_length = std::numeric_limits<double>::max()) const;
+  bool getPreviousShoulderLanelet(
+    const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const;
+  lanelet::ConstLanelets getShoulderLaneletSequenceUpTo(
+    const lanelet::ConstLanelet & lanelet,
+    const double min_length = std::numeric_limits<double>::max()) const;
+  lanelet::ConstLanelets getPreviousLaneletSequence(
+    const lanelet::ConstLanelets & lanelet_sequence) const;
+  lanelet::ConstLanelets getClosestLaneletSequence(const Pose & pose) const;
+  lanelet::ConstLanelets getLaneChangeTargetLanes(const Pose & pose) const;
+  lanelet::ConstLanelets getLaneSequenceUpTo(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getLaneSequenceAfter(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getLaneSequence(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getNeighborsWithinRoute(const lanelet::ConstLanelet & lanelet) const;
+  std::vector<lanelet::ConstLanelets> getLaneSection(const lanelet::ConstLanelet & lanelet) const;
+  lanelet::ConstLanelets getNextLaneSequence(const lanelet::ConstLanelets & lane_sequence) const;
+
+  // for path
+
+  PathWithLaneId updatePathTwist(const PathWithLaneId & path) const;
+};
+}  // namespace route_handler
+#endif  // ROUTE_HANDLER__ROUTE_HANDLER_HPP_

planning/route_handler/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>route_handler</name>
+  <version>0.1.0</version>
+  <description>The route_handling package</description>
+  <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
+  <license>Apache License 2.0</license>
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>