planning.hpp

Go to the documentation of this file.

#pragma once
 
#include <yaml-cpp/yaml.h>
 
#include <chrono>
#include <functional>
#include <map>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <vector>
 
#include "common_lib/communication/interfaces.hpp"
#include "common_lib/communication/marker.hpp"
#include "common_lib/competition_logic/mission_logic.hpp"
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/path_point.hpp"
#include "config/planning_config.hpp"
#include "custom_interfaces/msg/cone_array.hpp"
#include "custom_interfaces/msg/path_point.hpp"
#include "custom_interfaces/msg/path_point_array.hpp"
#include "custom_interfaces/msg/point2d.hpp"
#include "custom_interfaces/msg/point_array.hpp"
#include "custom_interfaces/msg/pose.hpp"
#include "planning/path_calculation.hpp"
#include "planning/skidpad.hpp"
#include "planning/smoothing.hpp"
#include "planning/velocity_planning.hpp"
#include "rcl_interfaces/srv/get_parameters.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_srvs/srv/trigger.hpp"
 
using PathPoint = common_lib::structures::PathPoint;
using Pose = common_lib::structures::Pose;
using Mission = common_lib::competition_logic::Mission;
 
class Planning : public rclcpp::Node {
public:
  explicit Planning(const PlanningParameters &params);
 
  static PlanningParameters load_config(std::string &adapter);
 
  void set_mission(Mission mission);
 
  friend class PacSimAdapter;
  friend class EufsAdapter;
  friend class FsdsAdapter;
  friend class VehicleAdapter;
 
private:
  /*--------------------- Mission and Configuration --------------------*/
  Mission mission_ = Mission::NONE;
  PlanningConfig planning_config_;
  Pose pose_;
  std::string map_frame_id_;
  double desired_velocity_;
  double initial_car_orientation_;
  int lap_counter_ = 0;
 
  /*--------------------- Planning Modules --------------------*/
  PathCalculation path_calculation_;
  PathSmoothing path_smoothing_;
  VelocityPlanning velocity_planning_;
  Skidpad skidpad_;
 
  /*--------------------- State Tracking --------------------*/
  bool is_braking_ = false;
  bool has_received_track_ = false;
  bool has_received_pose_ = false;
  bool is_path_final_ = false;
  bool is_path_closed_ = false;
  std::chrono::steady_clock::time_point brake_time_;
 
  /*--------------------- Path Data --------------------*/
 
  std::vector<PathPoint> full_path_;
  std::vector<PathPoint> smoothed_path_;
  std::vector<Cone> cone_array_;
 
  /*--------------------- Subscriptions --------------------*/
  rclcpp::Subscription<custom_interfaces::msg::Pose>::SharedPtr vehicle_localization_sub_;
  rclcpp::Subscription<custom_interfaces::msg::ConeArray>::SharedPtr track_map_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr lap_counter_sub_;
 
  /*--------------------- Publishers --------------------*/
  rclcpp::Publisher<custom_interfaces::msg::PathPointArray>::SharedPtr path_pub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr planning_execution_time_pub_;
 
  /*--------------------- Visualization Publishers --------------------*/
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr yellow_cones_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr blue_cones_pub_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr triangulations_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr path_to_car_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr full_path_pub_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr smoothed_path_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr velocity_hover_pub_;
 
  /*--------------------- Service Clients --------------------*/
  rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr param_client_;
 
  /*--------------------- Callbacks --------------------*/
  void vehicle_localization_callback(const custom_interfaces::msg::Pose &message);
 
  void track_map_callback(const custom_interfaces::msg::ConeArray &message);
 
  /*--------------------- Mission-Specific Planning --------------------*/
  void compute_path_orientation(std::vector<PathPoint> &path);
  void run_full_map();
  void run_acceleration();
 
  void run_autocross();
 
  void run_trackdrive();
 
  /*--------------------- Core Planning Operations --------------------*/
  void fetch_discipline();
 
  void run_planning_algorithms();
 
  /*--------------------- Publishing --------------------*/
  void publish_path_points() const;
 
  void publish_visualization_msgs() const;
 
  void publish_execution_time(rclcpp::Time start_time);
 
  /*--------------------- Abstract Methods --------------------*/
  virtual void finish() = 0;
};