lanelet2_route_planning.hpp

Go to the documentation of this file.

// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University
// SPDX-License-Identifier: Apache-2.0
 
#pragma once
 
#include <limits>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
 
#include <lanelet2_routing/Route.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>
#include <lanelet2_map_interface/lanelet2_map_interface.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <route_planning_msgs/action/plan_route.hpp>
#include <route_planning_msgs/msg/route.hpp>
 
namespace lanelet2_route_planning {
 
template <typename C>
struct is_vector : std::false_type {};
 
template <typename T, typename A>
struct is_vector<std::vector<T, A>> : std::true_type {};
 
template <typename C>
inline constexpr bool is_vector_v = is_vector<C>::value;
 
struct TopicDiagnosticConfig {
  double min_frequency = 0.0;
 
  double max_frequency = std::numeric_limits<double>::infinity();
 
  double min_acceptable_timestamp_delta = 0.0;
 
  double max_acceptable_timestamp_delta = std::numeric_limits<double>::infinity();
};
 
class Lanelet2RoutePlanning : public rclcpp::Node {
 public:
  Lanelet2RoutePlanning();
 
 private:
  template <typename T>
  void declareAndLoadParameter(const std::string& name,
                               T& param,
                               const std::string& description,
                               const bool add_to_auto_reconfigurable_params = true,
                               const bool is_required = false,
                               const bool read_only = false,
                               const std::optional<double>& from_value = std::nullopt,
                               const std::optional<double>& to_value = std::nullopt,
                               const std::optional<double>& step_value = std::nullopt,
                               const std::string& additional_constraints = "");
 
  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter>& parameters);
 
  void setup();
 
  bool checkMap(bool handle_update);
 
  bool buildRoutingGraph();
 
  void egoDataCallback(const perception_msgs::msg::EgoData::SharedPtr msg);
 
  void publishTimerCallback();
 
  void globalRoutePublishTimerCallback();
 
  rclcpp_action::GoalResponse actionHandleGoal(const rclcpp_action::GoalUUID& uuid,
                                               std::shared_ptr<const route_planning_msgs::action::PlanRoute::Goal> goal);
 
  rclcpp_action::CancelResponse actionHandleCancel(
      const std::shared_ptr<rclcpp_action::ServerGoalHandle<route_planning_msgs::action::PlanRoute>> goal_handle);
 
  void actionHandleAccepted(
      const std::shared_ptr<rclcpp_action::ServerGoalHandle<route_planning_msgs::action::PlanRoute>> goal_handle);
 
  void actionExecute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<route_planning_msgs::action::PlanRoute>> goal_handle);
 
  bool planRoute(const geometry_msgs::msg::PointStamped& destination,
                 const std::vector<geometry_msgs::msg::PointStamped>& intermediate_destinations = {});
 
  void buildGlobalRouteMessage();
 
  void buildEnrichedRouteMessage();
 
  void publishHealth(const unsigned char status, const std::string& msg, const rclcpp::Time& now);
 
  std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)>>> auto_reconfigurable_params_;
 
  OnSetParametersCallbackHandle::SharedPtr parameters_callback_;
 
  rclcpp::Subscription<perception_msgs::msg::EgoData>::SharedPtr subscriber_ego_data_;
 
  rclcpp::Publisher<route_planning_msgs::msg::Route>::SharedPtr publisher_route_;
 
  rclcpp::Publisher<route_planning_msgs::msg::Route>::SharedPtr publisher_global_route_;
 
  rclcpp::TimerBase::SharedPtr publish_timer_;
 
  rclcpp::TimerBase::SharedPtr global_route_publish_timer_;
 
  rclcpp_action::Server<route_planning_msgs::action::PlanRoute>::SharedPtr action_server_;
 
  route_planning_msgs::action::PlanRoute::Feedback::SharedPtr action_feedback_;
 
  route_planning_msgs::action::PlanRoute::Result::SharedPtr action_result_;
 
  std::shared_ptr<rclcpp_action::ServerGoalHandle<route_planning_msgs::action::PlanRoute>> action_goal_handle_;
 
  rclcpp::CallbackGroup::SharedPtr action_callback_group_;
 
  rclcpp::Time action_start_time_;
 
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 
  perception_msgs::msg::EgoData latest_ego_data_;
 
  std::unique_ptr<Lanelet2MapInterface> ll2_interface_;
 
  lanelet::routing::RoutingGraphUPtr routing_graph_;
 
  geometry_msgs::msg::Point starting_point_;
 
  geometry_msgs::msg::Point destination_;
 
  std::vector<geometry_msgs::msg::Point> intermediate_destinations_;
 
  lanelet::routing::Route latest_route_;
 
  bool has_enriched_route_ = false;
 
  bool is_publishing_route_;
 
  route_planning_msgs::msg::Route latest_route_msg_;
 
  route_planning_msgs::msg::Route latest_full_route_msg_;
 
  route_planning_msgs::msg::Route latest_global_route_msg_;
 
  std::vector<Eigen::Vector2d> latest_reference_line_;
 
  std::vector<size_t> latest_lanelet_idx_by_reference_line_point_idx_;
 
  std::vector<std::vector<int>> latest_suggested_turn_signal_distance_ahead_by_route_element_by_lane_element_;
 
  std::string ll2_map_server_name_ = "lanelet2_map_server";
 
  double publish_frequency_ = 10.0;
 
  double action_feedback_frequency_ = 1.0;
 
  double publish_frequency_global_ = 1.0;
 
  double sampling_distance_ = 1.0;
 
  double sampling_max_lateral_error_global_ = 0.5;
 
  bool project_destination_to_reference_line_ = true;
 
  double destination_distance_threshold_ = 1.0;
 
  double required_traveled_distance_proportion_ = 0.5;
 
  double enrich_route_ahead_ego_distance_ = 100.0;
 
  double enrich_route_behind_ego_distance_ = 10.0;
 
  double route_undershoot_distance_ = 0.0;
 
  double route_overshoot_distance_ = 0.0;
 
  double max_drivable_space_radius_ = 50.0;
 
  int max_num_threads_ = 0;
 
  double transform_timeout_ = 0.02;
 
  std::map<std::string, std::string> health_kv_;
 
  mutable std::mutex health_kv_mutex_;
 
  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr health_diagnostic_pub_;
 
  diagnostic_updater::Updater diagnostic_updater_;
 
  std::unique_ptr<diagnostic_updater::TopicDiagnostic> ego_data_diagnostic_;
 
  std::unique_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> route_timer_diagnostic_;
 
  std::unique_ptr<diagnostic_updater::HeaderlessTopicDiagnostic> global_route_timer_diagnostic_;
 
  TopicDiagnosticConfig ego_data_diagnostic_config_ = {45.45, 55.55, 0.0, 0.002};
 
  TopicDiagnosticConfig route_timer_diagnostic_config_ = {18.18, 22.22};
 
  TopicDiagnosticConfig global_route_timer_diagnostic_config_ = {0.91, 1.11};
};
 
}  // namespace lanelet2_route_planning