AutoPilotNode.hpp

Go to the documentation of this file.

#ifndef AUTOPILOT_NODE_HPP
#define AUTOPILOT_NODE_HPP
 
#include "galaxis/GalaxisNode.hpp"
#include "galaxis/messages.hpp"
#include "utils/clock.hpp"
 
#include <sensor_msgs/msg/image.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <image_transport/image_transport.h>
 
#include "Decision/AutoPilot.hpp"
 
#include "com/SharedMemory.hpp"
 
#include "nlohmann/json.hpp"
 
#include <std_msgs/msg/u_int8.hpp>
 
using namespace galaxis::utils;
using json = nlohmann::json;
 
class AutoPilotNode : public GalaxisNode {
private:
  bool m_simulationMode = false;
  bool m_debugMode = false;
  bool m_fakeLineDetectionMode = false;
  bool m_sendFullMapDebugMsg = false;
  bool m_laneDetectionSharedMemoryMode = true;
  bool m_sendLaneDetectionImage = false;
 
  int m_controllerReferenceSendSleepTimeMicroSec = 10000; // 100 FPS
  int m_poseSendSleepTimeMicroSec = 10000; // 100 FPS
 
  std::atomic<uint8_t> m_lastParkingState;
 
  rclcpp::Time m_lastLaneDetectionUpdate;
 
  AutoPilot m_autopilot;
 
  // ROS2
  //  Publisher
  rclcpp::Publisher<PathpolynomType>::SharedPtr m_pathpolynomPubSPtr;
  rclcpp::Publisher<ControllerReferenceType>::SharedPtr m_controllerRefPubSPtr;
  rclcpp::Publisher<CarPoseEstimatedType>::SharedPtr m_carposePubSPtr;
  rclcpp::Publisher<ParkingOperationType>::SharedPtr m_parkingOperationPubSPtr;
  rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr m_autopilotStatePubSPtr;
 
  //  Subscriber
  rclcpp::Subscription<MeasurementsType>::SharedPtr m_measurementSub;
  rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr m_subscriberCarPoseSPtr;
  rclcpp::Subscription<ObstacleListType>::SharedPtr m_subscriberObstaclesSPtr;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr m_subscriberLaneDetectionSub;
  rclcpp::Subscription<QRCodeDetectType>::SharedPtr m_subscriberQRCodeSPtr;
  rclcpp::Subscription<StopLineType>::SharedPtr m_subscriberStopLine;
  rclcpp::Subscription<ParkingStateType>::SharedPtr m_subscriberParkingStateSPtr;
  // image_transport::Subscriber m_vciLaneDetectionSub;
 
  // Debug
  image_transport::Publisher m_mapDataImageDebugPub;
  image_transport::Publisher m_laneDetectionMessage;
  image_transport::Publisher m_fullMapDataImageDebugPub;
  // image_transport::Publisher m_driveLaneImageDebugPub;
 
  std::shared_ptr<DriveInformation_t> m_currentDriveInformationSPtr = nullptr;
 
  std::mutex m_controllerPublishMtx;
 
  com::SharedMemory m_laneDetectionSharedMemory;
  size_t m_laneDetectionImageSize = 256 * 256 * 3; // width * height * sizeof(rgb_pixel) // ON change: also change in lane_detection_shared_memory_loop
 
 
  bool m_qrCodeDetectedSomeTime = false;
 
  std::atomic<float> m_stoplineIgnoreAreaRadius;
 
 
public:
  AutoPilotNode();
  ~AutoPilotNode();
 
private:
  void on_system_settings_callback(SystemSettingsType::SharedPtr msg);
 
  // loops
  void car_pose_publish_loop();
  void debug_publish_loop();
  void controller_reference_publish_loop();
  void lane_detection_shared_memory_loop();
 
  // simulation mode callbacks
  void on_simulation_pose_callback(geometry_msgs::msg::Pose::SharedPtr p_message);
  
  // standard mode callbacks
  void on_ecu_measurement_callback(MeasurementsType::SharedPtr msg);
  void on_vci_lane_detection_callback(sensor_msgs::msg::Image::SharedPtr msg);
  void on_obstacle_callback(ObstacleListType::SharedPtr msg);
  void on_qr_code_callback(QRCodeDetectType::SharedPtr msg);
  void on_stop_line_callback(StopLineType::SharedPtr msg);
  void on_parking_state_callback(ParkingStateType::SharedPtr msg);
 
  void send_controller_references();
 
  // debug
  void send_map_debug_image();
  void send_full_map_debug_image();
 
  rcl_interfaces::msg::SetParametersResult new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
};
 
#endif // AUTOPILOT_NODE_HPP