slam_node.hpp

Go to the documentation of this file.

#pragma once
 
#include <gtest/gtest_prod.h>
#include <tf2_ros/transform_broadcaster.h>
 
#include <Eigen/Dense>
#include <memory>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <string>
#include <visualization_msgs/msg/marker_array.hpp>
 
#include "common_lib/competition_logic/mission_logic.hpp"
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/pose.hpp"
#include "common_lib/structures/velocities.hpp"
#include "custom_interfaces/msg/cone_array.hpp"
#include "custom_interfaces/msg/perception_output.hpp"
#include "custom_interfaces/msg/point2d.hpp"
#include "custom_interfaces/msg/pose.hpp"
#include "custom_interfaces/msg/velocities.hpp"
#include "perception_sensor_lib/data_association/maximum_likelihood_md.hpp"
#include "perception_sensor_lib/data_association/maximum_likelihood_nll.hpp"
#include "rclcpp/rclcpp.hpp"
#include "slam_config/general_config.hpp"
#include "slam_solver/slam_solver.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
 
class SLAMNode : public rclcpp::Node {
protected:
  rclcpp::CallbackGroup::SharedPtr _callback_group_;  
  rclcpp::Subscription<custom_interfaces::msg::PerceptionOutput>::SharedPtr
      _perception_subscription_;
  rclcpp::Subscription<custom_interfaces::msg::Velocities>::SharedPtr _velocities_subscription_;
  rclcpp::Publisher<custom_interfaces::msg::Pose>::SharedPtr _vehicle_pose_publisher_;
  rclcpp::Publisher<custom_interfaces::msg::ConeArray>::SharedPtr _map_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr _visualization_map_publisher_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr
      _position_visualization_publisher_;  
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
      _trajectory_visualization_publisher_;  
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
      _associations_visualization_publisher_;
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr _execution_time_publisher_;
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr
      _covariance_publisher_;  
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr _lap_counter_publisher_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr _perception_delta_publisher_;
  rclcpp::Time _last_perception_message_time_ = rclcpp::Time(0);
  rclcpp::CallbackGroup::SharedPtr
      _parallel_callback_group_;  // Group of callbacks that runs in parallel with all others, used
                                  // for EKF SLAM with parallel correction step
  std::shared_mutex mutex_;
 
  std::shared_ptr<tf2_ros::TransformBroadcaster> _tf_broadcaster_;
  rclcpp::TimerBase::SharedPtr _timer_;      
  std::shared_ptr<SLAMSolver> _slam_solver_; 
  std::vector<common_lib::structures::Cone> _perception_map_;
  common_lib::structures::Velocities _vehicle_state_velocities_;
  std::vector<common_lib::structures::Cone> _track_map_;
  Eigen::VectorXi _associations_;
  Eigen::VectorXd _observations_global_; 
  Eigen::VectorXd _map_coordinates_;     
  common_lib::structures::Pose _vehicle_pose_;
  std::shared_ptr<std::vector<double>>
      _execution_times_;  //< Execution times: 0 -> total motion;
                          // 1 -> total observation; the rest are solver specific
  common_lib::competition_logic::Mission _mission_ = common_lib::competition_logic::Mission::NONE;
  bool _go_ = true;  
  std::string _adapter_name_;
  std::string _vehicle_frame_id_;  
  SLAMParameters _params_;
 
  void _perception_subscription_callback(const custom_interfaces::msg::PerceptionOutput& msg);
 
  void _velocities_subscription_callback(const custom_interfaces::msg::Velocities& msg);
 
  void _imu_subscription_callback(const sensor_msgs::msg::Imu& msg);
 
  void _publish_vehicle_pose();
 
  void _publish_map();
 
  void _publish_associations();
 
  void _publish_covariance();
 
  void _publish_lap_counter();
 
  bool _is_mission_finished();
 
public:
  // /**
  //  * @brief Constructor of the main node, most things are received by launch parameter
  //  */
  // SLAMNode();
 
  SLAMNode(const SLAMParameters& params);
 
  void init();
 
  virtual void finish() = 0;
};