plan_route_action_client.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 <memory>
#include <optional>
#include <string>
#include <utility>
#include <vector>
 
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.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>
 
namespace plan_route_action_client {
 
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;
 
class PlanRouteActionClient : public rclcpp::Node {
  using PlanRoute = route_planning_msgs::action::PlanRoute;
  using GoalHandlePlanRoute = rclcpp_action::ClientGoalHandle<PlanRoute>;
 
 public:
  PlanRouteActionClient();
 
 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();
 
  void goalPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
 
  void autoPlanningTimerCallback();
 
  void planToNextWaypoint();
 
  void planToRandomDestination();
 
  void sendGoal(const geometry_msgs::msg::PoseStamped::SharedPtr msg,
                const std::vector<geometry_msgs::msg::PointStamped>& intermediate_destinations = {});
 
  void goalResponseCallback(const GoalHandlePlanRoute::SharedPtr& goal_handle);
 
  void feedbackCallback(GoalHandlePlanRoute::SharedPtr goal_handle, const std::shared_ptr<const PlanRoute::Feedback> feedback);
 
  void resultCallback(const GoalHandlePlanRoute::WrappedResult& result);
 
  std::vector<std::tuple<std::string, std::function<void(const rclcpp::Parameter&)>>> auto_reconfigurable_params_;
 
  OnSetParametersCallbackHandle::SharedPtr parameters_callback_;
 
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_subscriber_;
 
  rclcpp_action::Client<PlanRoute>::SharedPtr action_client_;
 
  std::shared_future<GoalHandlePlanRoute::SharedPtr> goal_handle_future_;
 
  rclcpp::TimerBase::SharedPtr auto_planning_timer_;
 
  std::unique_ptr<Lanelet2MapInterface> ll2_interface_;
 
  std::vector<std::pair<double, double>> waypoints_;
 
  std::vector<double> waypoint_wait_times_;
 
  size_t next_waypoint_idx_ = 0;
 
  bool has_active_waypoint_ = false;
 
  double active_waypoint_wait_time_s_ = 0.0;
 
  double auto_planning_resume_time_s_ = 0.0;
 
  bool has_completed_one_goal_ = false;
 
  std::string ll2_map_server_name_ = "ll2_map_server";
 
  std::vector<std::string> waypoints_param_;
 
  bool enable_random_destination_ = false;
 
  bool enable_continuous_planning_ = false;
 
  bool cancel_route_ = false;
};
 
}  // namespace plan_route_action_client