perception_node.hpp

Go to the documentation of this file.

#pragma once
 
#include <cone_evaluator/cone_evaluator.hpp>
#include <cone_validator/cylinder_validator.hpp>
#include <cone_validator/deviation_validator.hpp>
#include <cone_validator/displacement_validator.hpp>
#include <cone_validator/npoints_validator.hpp>
#include <cone_validator/z_score_validator.hpp>
#include <string>
#include <unordered_map>
#include <utils/ground_grid.hpp>
#include <utils/plane.hpp>
#include <utils/trimming_parameters.hpp>
#include <vector>
 
#include "clustering/dbscan.hpp"
#include "clustering/grid_clustering.hpp"
#include "common_lib/communication/marker.hpp"
#include "common_lib/competition_logic/mission_logic.hpp"
#include "common_lib/config_load/config_load.hpp"
#include "common_lib/structures/velocities.hpp"
#include "cone_differentiation/least_squares_differentiation.hpp"
#include "cone_validator/height_validator.hpp"
#include "custom_interfaces/msg/cone_array.hpp"
#include "custom_interfaces/msg/operational_status.hpp"
#include "custom_interfaces/msg/perception_output.hpp"
#include "custom_interfaces/msg/velocities.hpp"
#include "deskew/deskew.hpp"
#include "fov_trimming/acceleration_trimming.hpp"
#include "fov_trimming/cut_trimming.hpp"
#include "fov_trimming/skidpad_trimming.hpp"
#include "ground_removal/himmelsbach.hpp"
#include "ground_removal/ransac.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
#include "std_msgs/msg/header.hpp"
#include "std_srvs/srv/empty.hpp"
#include "std_srvs/srv/trigger.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "wall_removal/grid_removal.hpp"
#include "wall_removal/wall_removal.hpp"
#include "yaml-cpp/yaml.h"
 
using Mission = common_lib::competition_logic::Mission;
 
struct PerceptionParameters {
  std::string vehicle_frame_id_;  
  std::string adapter_;           
  uint8_t default_mission_;
  bool publish_fov_trimmed_cloud;
  bool publish_ground_removed_cloud;
  bool publish_wall_removed_cloud;
  std::shared_ptr<std::unordered_map<int16_t, std::shared_ptr<FovTrimming>>> fov_trim_map_;
  std::shared_ptr<GroundGrid> ground_grid_;        
  std::shared_ptr<GroundRemoval> ground_removal_;  
  std::shared_ptr<WallRemoval> wall_removal_;      
  std::shared_ptr<Clustering> clustering_;         
  std::shared_ptr<LeastSquaresDifferentiation>
      cone_differentiator_;  
  std::shared_ptr<ConeEvaluator> cone_evaluator_;  
  std::unordered_map<std::string, double>
      weight_values;  
};
 
class Perception : public rclcpp::Node {
public:
  explicit Perception(const PerceptionParameters& params);
 
  static PerceptionParameters load_config();
 
  void point_cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
 
private:
  std::string _vehicle_frame_id_;       
  std::string _adapter_;                
  int16_t _mission_type_;               
  bool _publish_fov_trimmed_cloud_;     
  bool _publish_ground_removed_cloud_;  
  bool _publish_wall_removed_cloud_;    
  std::shared_ptr<std::unordered_map<int16_t, std::shared_ptr<FovTrimming>>>
      _fov_trim_map_;                               
  std::shared_ptr<GroundGrid> _ground_grid_;        
  std::shared_ptr<GroundRemoval> _ground_removal_;  
  std::shared_ptr<WallRemoval> _wall_removal_;      
  std::shared_ptr<Clustering> _clustering_;         
  std::shared_ptr<ConeDifferentiation>
      _cone_differentiator_;  
  std::shared_ptr<ConeEvaluator> _cone_evaluator_;  
  std::shared_ptr<Deskew> _deskew_;                 
  rclcpp::Subscription<custom_interfaces::msg::OperationalStatus>::SharedPtr
      _operational_status_subscription;  
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr
      _point_cloud_subscription;  
  rclcpp::Subscription<custom_interfaces::msg::Velocities>::SharedPtr _velocities_subscription_;
  rclcpp::Publisher<custom_interfaces::msg::PerceptionOutput>::SharedPtr
      _cones_publisher;  
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
      _cone_marker_array_;  
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr
      _perception_execution_time_publisher_;  
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
      _fov_trimmed_publisher_;  
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
      _ground_removed_publisher_;  
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr
      _wall_removed_publisher_;  
  common_lib::structures::Velocities
      _vehicle_velocity_;  // Last received vehicle velocity, used to deskew the point cloud
 
  rclcpp::TimerBase::SharedPtr lidar_off_timer_;  
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr emergency_client_;
  std::shared_ptr<std::vector<double>> _execution_times_;  
 
  void publish_cones(std::vector<Cluster>* cones, double exec_time, rclcpp::Time start_time);
 
  void velocities_callback(const custom_interfaces::msg::Velocities& msg);
 
  void lidar_timer_callback();
};