se_node.hpp

Go to the documentation of this file.

#pragma once
 
#include <gtest/gtest_prod.h>
 
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <typeinfo>
 
#include "adapter_ekf_state_est/eufs.hpp"
#include "adapter_ekf_state_est/fsds.hpp"
#include "common_lib/competition_logic/mission_logic.hpp"
#include "common_lib/car_parameters/car_parameters.hpp"
#include "custom_interfaces/msg/cone_array.hpp"
#include "custom_interfaces/msg/point2d.hpp"
#include "custom_interfaces/msg/vehicle_state.hpp"
#include "eufs_msgs/msg/wheel_speeds_stamped.hpp"
#include "kalman_filter/ekf.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
 
class Adapter;
 
class SENode : public rclcpp::Node {
  rclcpp::Subscription<custom_interfaces::msg::ConeArray>::SharedPtr _perception_subscription_;
  rclcpp::Publisher<custom_interfaces::msg::VehicleState>::SharedPtr _vehicle_state_publisher_;
  rclcpp::Publisher<custom_interfaces::msg::VehicleState>::SharedPtr _vehicle_state_publisher_wss_;
  rclcpp::Publisher<custom_interfaces::msg::VehicleState>::SharedPtr _vehicle_state_publisher_imu_;
  rclcpp::Publisher<custom_interfaces::msg::ConeArray>::SharedPtr _map_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr _visualization_map_publisher_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr _correction_execution_time_publisher_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr _prediction_execution_time_publisher_;
  rclcpp::TimerBase::SharedPtr _timer_;        
  std::shared_ptr<ExtendedKalmanFilter> _ekf_; 
  std::shared_ptr<std::vector<common_lib::structures::Cone>> _perception_map_;
  std::shared_ptr<MotionUpdate> _motion_update_;
  std::shared_ptr<std::vector<common_lib::structures::Cone>> _track_map_;
  std::shared_ptr<common_lib::structures::VehicleState> _vehicle_state_;
  common_lib::competition_logic::Mission _mission_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr _position_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr _car_model_publisher_;
  bool _go_;  
  bool _use_odometry_;
  bool _use_simulated_perception_;
  std::string _adapter_name_;
  std::shared_ptr<Adapter> _adapter_;
  std::mutex _mutex_;  
  std::shared_ptr<common_lib::car_parameters::CarParameters> _car_parameters_ = std::make_shared<common_lib::car_parameters::CarParameters>();
 
  void _perception_subscription_callback(const custom_interfaces::msg::ConeArray& msg);
 
  void _imu_subscription_callback(const sensor_msgs::msg::Imu& imu_msg);
 
  void _wheel_speeds_subscription_callback(double rl_speed, double rr_speed, double fl_speed,
                                           double fr_speed, double steering_angle,
                                           const rclcpp::Time& timestamp);
 
  void _update_and_publish();
 
  void _publish_vehicle_state();
 
  void _publish_vehicle_state_wss();
 
  void _publish_vehicle_state_imu();
 
  void _publish_map();
 
  void _ekf_step();
 
public:
  SENode();
 
  friend class Adapter;
  friend class EufsAdapter;
  friend class FsdsAdapter;
  friend class PacsimAdapter;
  friend class VehicleAdapter;
};