AutoPilotNode.cpp

Go to the documentation of this file.

#include "Node/AutoPilotNode.hpp"
 
#include "utils/logger.hpp"
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include <galaxis/AutopilotSettings.hpp>
#include <utils/file.hpp>
#include <utils/settings.hpp>
#include <cstdlib>
#include <sensor_msgs/image_encodings.hpp>
 
using std::placeholders::_1;
using namespace galaxis::ros;
using namespace galaxis::utils;
 
using json = nlohmann::json;
 
 
AutoPilotNode::AutoPilotNode() : GalaxisNode("Autopilot"), m_autopilot(this), m_laneDetectionSharedMemory("/galaxis__lanes") {
  // Map Messages: http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html
  log("AutoPilotNode: Initializing...");
 
  // Settings
  try {
    std::string configPath = "settings/onstartup_autopilot.json";
    auto settingsJsonSPtr = read_json_file(configPath);
    json& settingsJson = (*settingsJsonSPtr)["settings"];
 
    m_simulationMode = this->get_global_settings()["simulation_mode"];
    m_fakeLineDetectionMode = this->get_global_settings()["fake_line_detection_mode"];
    m_debugMode = settingsJson["debug_mode"];
    m_sendFullMapDebugMsg = settingsJson["send_full_map_debug_msg"];
    m_laneDetectionSharedMemoryMode = settingsJson["lane_detection_shared_memory"];
    m_sendLaneDetectionImage = settingsJson["send_lane_detection_image"];
    m_autopilot.m_decision.m_stopAtStartLine = settingsJson["wait_at_startline"];
 
    if(settingsJson["wait_for_qr_code_detection"] == false) { // start driving instantly
      m_autopilot.m_decision.start_driving();
    }
  }
  catch (const std::runtime_error& error) {
    // pass
    log_warning("##########################################################");
    log_warning("############## READING CONFIGURATION FAILED ##############");
    log_warning("############# Falling BACK TO DEFAULT CONFIG #############");
    log_warning("##########################################################");
    sleep(10);
  }
 
  log("Configuration:\n - SimulationMode: ", m_simulationMode ? "ENABLED" : "DISABLED");
  log(" - DebugMode: ", m_debugMode ? "ENABLED" : "DISABLED");
  log(" - FakeLineDetectionMode: ", m_fakeLineDetectionMode ? "ENABLED" : "DISABLED");
  log(" - SendFullMapDebugMessage: ", m_sendFullMapDebugMsg ? "ENABLED" : "DISABLED");
  log(" - PoseSendSleepTime [microsec]: ", m_poseSendSleepTimeMicroSec);
  if(m_debugMode || m_simulationMode || m_fakeLineDetectionMode || m_sendFullMapDebugMsg) sleep(1); // To view the configurations
 
  // Publisher
  galaxis::ros::create_publisher(this, m_pathpolynomPubSPtr);
  galaxis::ros::create_publisher(this, m_controllerRefPubSPtr);
  galaxis::ros::create_publisher(this, m_carposePubSPtr);
  galaxis::ros::create_publisher(this, m_parkingOperationPubSPtr);
 
  if(m_debugMode) {
    m_mapDataImageDebugPub = image_transport::create_publisher(this, TOPIC_AP_DATA_IMAGE_DEBUG);
    rclcpp::QoS qos_params_situation = get_qos_profile(QOS_RELIABLE);
    m_autopilotStatePubSPtr = this->create_publisher<std_msgs::msg::UInt8>("/galaxis/ap/situation_state", qos_params_situation);
  }
  if(m_sendLaneDetectionImage) m_laneDetectionMessage = image_transport::create_publisher(this, "/galaxis/debug/ap/shm/lane_image");
  if(m_sendFullMapDebugMsg) m_fullMapDataImageDebugPub = image_transport::create_publisher(this, TOPIC_AP_FULL_MAP_DATA_IMAGE_DEBUG);
 
  if(m_simulationMode) { // Initializing Simulation
    rclcpp::QoS qos_params = get_qos_profile(QOS_BEST_EFFORT);
    m_subscriberLaneDetectionSub = this->create_subscription<sensor_msgs::msg::Image>(
      galaxis::ros::TOPIC_VCI_LANE_IMAGE_SIMULATION,
      qos_params,
      std::bind(&AutoPilotNode::on_vci_lane_detection_callback, this, _1));
 
    // Car Pose Subscription (Position, Orientation)
    qos_params = get_qos_profile(QOS_BEST_EFFORT);
    m_subscriberCarPoseSPtr  = this->create_subscription<geometry_msgs::msg::Pose>("/simulation/gazebo/model/dr_drift/pose", qos_params, std::bind(&AutoPilotNode::on_simulation_pose_callback, this, std::placeholders::_1));
 
    // Obstacle Subscription
    galaxis::ros::create_subscription(this, m_subscriberObstaclesSPtr, std::bind(&AutoPilotNode::on_obstacle_callback, this, _1));
 
    if(m_fakeLineDetectionMode) { // for SimulationLineDetection 
      m_autopilot.m_mapDataLayer.setLaneImageConversionFactor(galaxis::autopilot::simulationPixelToPositionConversionFactorCM);
      
      // ignore isInImage function which cuts off overlaid section
      m_autopilot.m_mapDataLayer.enableSimulationMode();
    } else { // for VCI
      m_autopilot.m_mapDataLayer.setLaneImageConversionFactor(0.5f); // in centimeter
    }
 
    log_warning("SimulationLineDetection and Autopilot must run in same docker container!");
  } else { // Initializing Normal Mode
 
    galaxis::ros::create_subscription(this,
                                    m_measurementSub,
                                    std::bind(&AutoPilotNode::on_ecu_measurement_callback, this, _1));
 
 
    // lane image
    if(m_laneDetectionSharedMemoryMode == false) {
      rclcpp::QoS qos_params = get_qos_profile(QOS_BEST_EFFORT);
      m_subscriberLaneDetectionSub = this->create_subscription<sensor_msgs::msg::Image>(
        galaxis::ros::TOPIC_VCI_LANE_IMAGE_SIMULATION,
        qos_params,
        std::bind(&AutoPilotNode::on_vci_lane_detection_callback, this, _1));
    }
    else {
      m_laneDetectionSharedMemory.allocate(m_laneDetectionImageSize);
      std::thread(&AutoPilotNode::lane_detection_shared_memory_loop, this).detach();
    }
 
    // Obstacle Subscription
    galaxis::ros::create_subscription(this, m_subscriberObstaclesSPtr, std::bind(&AutoPilotNode::on_obstacle_callback, this, _1));
 
    // QRCode Subscription
    galaxis::ros::create_subscription(this, m_subscriberQRCodeSPtr, std::bind(&AutoPilotNode::on_qr_code_callback, this, _1));
 
    // StopLine Subscription
    galaxis::ros::create_subscription(this, m_subscriberStopLine, std::bind(&AutoPilotNode::on_stop_line_callback, this, _1));
 
    // Parking
    m_lastParkingState = 0;
    galaxis::ros::create_subscription(this, m_subscriberParkingStateSPtr, std::bind(&AutoPilotNode::on_parking_state_callback, this, _1));
    galaxis::ros::create_publisher(this, m_parkingOperationPubSPtr);
 
    rclcpp::Time timeNow = this->get_clock()->now();
    m_autopilot.m_localizer.add_position_timepoint(timeNow, Vector2f(0.f,0.f), 0, 0); // if no IMU is there
 
    m_autopilot.m_mapDataLayer.setLaneImageConversionFactor(galaxis::autopilot::realPixelToPositionConversionFactorCM);
  }
 
  m_lastLaneDetectionUpdate = this->get_clock()->now();
 
  this->enable_parameters(std::bind(&AutoPilotNode::new_parameters_callback, this, std::placeholders::_1));
 
  log("AutopilotNode: Waiting for messages...");
 
  std::thread(&AutoPilotNode::car_pose_publish_loop, this).detach();
  std::thread(&AutoPilotNode::controller_reference_publish_loop, this).detach();
  
  if(m_debugMode) {
    std::thread(&AutoPilotNode::debug_publish_loop, this).detach();
  }
 
  m_stoplineIgnoreAreaRadius = 0.f; // disabled
}
 
AutoPilotNode::~AutoPilotNode() {}
 
void AutoPilotNode::car_pose_publish_loop() {
  while(1) {
    usleep(m_poseSendSleepTimeMicroSec);
    if(m_autopilot.m_localizer.is_empty()) continue;
 
    auto localizationPointSPtr = m_autopilot.m_localizer.get_latest_position_timepoint();
    
    std::unique_ptr<CarPoseEstimatedType> carposeUPtr = std::make_unique<CarPoseEstimatedType>();
    carposeUPtr->pose.pos_x = localizationPointSPtr->position[0] / 1000;
    carposeUPtr->pose.pos_y = localizationPointSPtr->position[1] / 1000;
    carposeUPtr->pose.orientation = localizationPointSPtr->orientation;
    carposeUPtr->header.stamp = localizationPointSPtr->timestamp;
 
    m_carposePubSPtr->publish(std::move(carposeUPtr));
  }
}
 
void AutoPilotNode::controller_reference_publish_loop() {
  while(1) {
    MicroStopwatch stopwatch;
    send_controller_references();
    long long time = stopwatch.get_time();
    time = m_controllerReferenceSendSleepTimeMicroSec - time;
    if(time > 0) usleep(time);
  }
}
 
void AutoPilotNode::debug_publish_loop() {
  while(1) {
    m_controllerPublishMtx.lock();
    MicroStopwatch stopwatch;
    send_map_debug_image();
    if(m_sendFullMapDebugMsg) send_full_map_debug_image();
    m_controllerPublishMtx.unlock();
 
    // send autopilot state
    auto msgUPtr = std::make_unique<std_msgs::msg::UInt8>();
    msgUPtr->data = static_cast<uint8_t>(m_autopilot.m_decision.get_current_situationstate());
    m_autopilotStatePubSPtr->publish(std::move(msgUPtr));
 
    long long time = stopwatch.get_time();
    log("\033[33mAutoPilotNode [debug_publish_loop]: Sending Debug Messages [microsec]: ", time, "\033[0m");
    // sleep (~30hz)
    time = 33000 - time;
    if(time > 0) {
      usleep(time);
    }
  }
}
 
void AutoPilotNode::lane_detection_shared_memory_loop() {
  com::SharedData data;
 
  while(1) {
    if(m_laneDetectionSharedMemory.is_new_data_available()) {
      log("\033[44[AutoPilotNode] [lane_detection_shared_memory_loop] New lane message!\033[49");
 
      sensor_msgs::msg::Image::SharedPtr imageMsg = std::make_shared<sensor_msgs::msg::Image>();
      imageMsg->height = 256;
      imageMsg->width = 256;
      imageMsg->step = imageMsg->width * 3;
      imageMsg->data.resize(imageMsg->step * imageMsg->height);
      imageMsg->encoding = sensor_msgs::image_encodings::RGB8;
 
      data.data = imageMsg->data.data();
      data.dataSize = imageMsg->data.size();
 
      size_t dataSizeRead = m_laneDetectionSharedMemory.read_mutexed(data, m_laneDetectionImageSize);
      if(dataSizeRead == 0) {
        log_warning("[AutoPilotNode] Sth failed while reading shared memory!");
        continue;
      }
      else if(dataSizeRead != m_laneDetectionImageSize) {
        log_warning("[AutoPilotNode] Shared Memory: Failed to read whole image!");
      }
 
      imageMsg->header.stamp = rclcpp::Time(data.tv_sec, data.tv_nsec);
 
      on_vci_lane_detection_callback(imageMsg);
 
      if(m_sendLaneDetectionImage) m_laneDetectionMessage.publish(imageMsg);
    }
    else {
      std::this_thread::sleep_for(3ms);
    }
  }
}
 
void AutoPilotNode::on_simulation_pose_callback(geometry_msgs::msg::Pose::SharedPtr p_message) {
  rclcpp::Time currentTime = this->get_clock()->now(); // WARNING Pose has no header!
 
  // Copied from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
  // yaw (z-axis rotation)
  double siny_cosp = 2 * (p_message->orientation.w * p_message->orientation.z + p_message->orientation.x * p_message->orientation.y);
  double cosy_cosp = 1 - 2 * (p_message->orientation.y * p_message->orientation.y + p_message->orientation.z * p_message->orientation.z);
  double yaw = std::atan2(siny_cosp, cosy_cosp);
 
  double absoluteYOrientation = 2*MATH_PI_DOUBLE - yaw;
  // fix orientation from simulation to internal galaxis representation
  absoluteYOrientation += MATH_PI_DOUBLE / 2;
  absoluteYOrientation = std::fmod(absoluteYOrientation, 2*MATH_PI_DOUBLE);
 
  
  Vector2f position = Vector2f(p_message->position.x, p_message->position.y); // m
  // Shift Position to Front Axis
  Vector2f carPosOffToFrontAx = galaxis::autopilot::simluationCarPositionOffsetToFrontAxisM;
  Eigen::Rotation2Df rotationMat(-absoluteYOrientation);
  carPosOffToFrontAx = rotationMat * carPosOffToFrontAx;
  position += carPosOffToFrontAx;
  // position (m) to mm
  position *= 1000; // to mm
  // WARNING! currentTime Nanoseconds must not comply with on_vci_lane_detection_callback time; velocity set to 0!
  m_autopilot.m_localizer.add_position_timepoint(currentTime, position, m_autopilot.m_decision.get_velocity_reference() * 1000.f, absoluteYOrientation);
  
 
  // log_debug("CarPose added. Time: ", currentTime.nanoseconds());
}
 
void AutoPilotNode::on_ecu_measurement_callback(MeasurementsType::SharedPtr msg) {
  log("AutoPilotNode: Received Measurements Message");
  rclcpp::Time msgTimestamp = msg->header.stamp;
  // rclcpp::Time msgTimestamp = this->get_clock()->now();
 
  double orientation = msg->orientation;
  m_autopilot.m_localizer.add_state(msgTimestamp, galaxis::motorSettings::erpmToVelocity(msg->motor_erpm) * 1000, orientation);
 
  // time of flight right / rear
  m_autopilot.m_decision.update_tof(float(msg->time_of_flight_right) / 1000.f /*right*/, float(msg->time_of_flight_rear) / 1000.f/*rear*/);
}
 
void AutoPilotNode::on_vci_lane_detection_callback(sensor_msgs::msg::Image::SharedPtr msg) {
  log("AutoPilotNode: Received Lane Detection Message! Image Age [millisec]: ", (this->get_clock()->now() - msg->header.stamp).nanoseconds() / 1000000.f,"; Last Update was before [millisec]: ", (this->get_clock()->now() - m_lastLaneDetectionUpdate).nanoseconds() / 1000000.f);
  m_lastLaneDetectionUpdate = this->get_clock()->now();
 
  if(m_autopilot.m_localizer.is_empty()){
    log_warning("AutoPilotNode: Localizer never received Car Position. Skip.");
    return;
  }
 
  if(msg->encoding != sensor_msgs::image_encodings::RGB8) {
    log_warning("AutoPilotNode: Lane Detection Image Encoding wrong! Must be RGB8 (skipping frame)");
    return;
  }
 
  MicroStopwatch stopwatch;
 
  // Message Timestamp:
  rclcpp::Time msgTimestamp = msg->header.stamp;
 
  // TODO: check clock
  try {
    // auto localizationPointSPtr                        = m_autopilot.m_localizer.get_latest_position_timepoint(); // TODO
    auto localizationPointSPtr = m_autopilot.m_localizer.get_position_timepoint_by_time(msgTimestamp);
    galaxis::lina::Vector2f localizationPointPosition = localizationPointSPtr->position / 10; // update Map : divide position by 10 for mm to cm conversion
 
    if(m_debugMode == true) {
      auto localizationLookupDiffTime = msgTimestamp - localizationPointSPtr->timestamp;
      log_debug("AutoPilotNode: Lane Detection-Pose Lookup Diff Time [nanosec]: ", localizationLookupDiffTime.nanoseconds());
      log_debug("AutoPilotNode: Lookup Orientation [Rad]: ", localizationPointSPtr->orientation);
    }
 
    m_autopilot.m_mapDataLayer.updateMap(msg, localizationPointPosition, localizationPointSPtr->orientation);
  }
  catch (const std::runtime_error& error) {
    log_error("Runtime ERROR: ", error.what());
  }
  
  log("AutoPilotNode [on_vci_lane_detection_callback]: Map Data Layer update duration [microsec]: ", stopwatch.get_time());
 
  // this->send_controller_references();
}
 
void AutoPilotNode::send_controller_references() {
  MicroStopwatch stopwatch;
 
  m_controllerPublishMtx.lock();
  m_currentDriveInformationSPtr = m_autopilot.get_drive_information();
 
  m_controllerPublishMtx.unlock();
  log("\033[36mAutoPilotNode [send_controller_references]: Polynom Extraction [microsec]: ", stopwatch.get_time(), "\033[0m");
 
  std::unique_ptr<PathpolynomType> polyMsg = std::make_unique<PathpolynomType>();
 
  polyMsg->header.stamp = this->get_clock()->now();
  if(m_currentDriveInformationSPtr->polynom->size() == 4) {
    polyMsg->c[0] = m_currentDriveInformationSPtr->polynom->at(0);
    polyMsg->c[1] = m_currentDriveInformationSPtr->polynom->at(1);
    polyMsg->c[2] = m_currentDriveInformationSPtr->polynom->at(2);
    polyMsg->c[3] = m_currentDriveInformationSPtr->polynom->at(3);
  } else {
    polyMsg->c[0] = 0;
    polyMsg->c[1] = 0;
    polyMsg->c[2] = 0;
    polyMsg->c[3] = 0;
    log_error("[AutopilotNode] Polynom Wrong.");
  }
 
  if(m_currentDriveInformationSPtr->right != nullptr){
    for(uint32_t i = 0; i < m_currentDriveInformationSPtr->right->size(); i ++){
      polyMsg->c_r[i] = m_currentDriveInformationSPtr->right->at(i);
    }
 
    polyMsg->max_right = m_currentDriveInformationSPtr->rightPolyMax;
    polyMsg->min_right = m_currentDriveInformationSPtr->rightPolyMin;  
    polyMsg->right_enabled = true;
  }
  
  if(m_currentDriveInformationSPtr->center != nullptr){
    for(uint32_t i = 0; i < m_currentDriveInformationSPtr->center->size(); i ++){
      polyMsg->c_m[i] = m_currentDriveInformationSPtr->center->at(i);
    }
 
    polyMsg->max_center = m_currentDriveInformationSPtr->centerPolyMax;
    polyMsg->min_center = m_currentDriveInformationSPtr->centerPolyMin; 
    polyMsg->center_enabled = true;
  }
 
  if(m_currentDriveInformationSPtr->left != nullptr){
    for(uint32_t i = 0; i < m_currentDriveInformationSPtr->left->size(); i ++){
      polyMsg->c_l[i] = m_currentDriveInformationSPtr->left->at(i);
    }
 
    polyMsg->max_left = m_currentDriveInformationSPtr->leftPolyMax;
    polyMsg->min_left = m_currentDriveInformationSPtr->leftPolyMin; 
    polyMsg->left_enabled = true;
  }
  // Controller Reference
  std::unique_ptr<ControllerReferenceType> controllerRefMsgUPtr = std::make_unique<ControllerReferenceType>();
 
  controllerRefMsgUPtr->header.stamp = this->get_clock()->now();
  controllerRefMsgUPtr->velocity_reference = m_currentDriveInformationSPtr->velocityReference;
  controllerRefMsgUPtr->indicator_left = m_currentDriveInformationSPtr->indicatorLeft;
  controllerRefMsgUPtr->indicator_right = m_currentDriveInformationSPtr->indicatorRight;
  // controllerRefMsgUPtr->velocity_apply_raw = m_currentDriveInformationSPtr->startParking; // apply this velocity for parking
 
  std::unique_ptr<ParkingOperationType> parkingOperationUPtr = std::make_unique<ParkingOperationType>();
  parkingOperationUPtr->header = polyMsg->header;
  if(m_currentDriveInformationSPtr->startParking) {
    parkingOperationUPtr->operation = 2;
  }
  else {
    parkingOperationUPtr->operation = 1; // stop parking node
  }
 
  // publish at same time
  m_pathpolynomPubSPtr->publish(std::move(polyMsg));
  m_controllerRefPubSPtr->publish(std::move(controllerRefMsgUPtr));
  m_parkingOperationPubSPtr->publish(std::move(parkingOperationUPtr));
}
 
void AutoPilotNode::on_obstacle_callback(ObstacleListType::SharedPtr msg) {
  if(m_autopilot.m_localizer.is_empty()){
    log_warning("AutoPilotNode [on_obstacle_callback]: Obstacle CB - Localizer never received Car Position. Skip.");
    return;
  }
 
  if(msg->obstacles.size() <= 0) { return; }
 
  log("AutoPilotNode: Obstacle Message received. Size: ", msg->obstacles.size());
 
  rclcpp::Time msgTimestamp = msg->header.stamp;
  auto posTpSPtr = m_autopilot.m_localizer.get_position_timepoint_by_time(msgTimestamp);
  // auto posTpSPtr = m_autopilot.m_localizer.get_latest_position_timepoint();
  
  for(auto obstacleTypeObj : msg->obstacles) {
    m_autopilot.m_mapDataLayer.addOrUpdateStaticObstacle(obstacleTypeObj, posTpSPtr->position / 10, posTpSPtr->orientation, msgTimestamp);
  }
}
 
void AutoPilotNode::on_qr_code_callback(QRCodeDetectType::SharedPtr msg) {
  log_debug("AutoPilotNode: QRCode Message received. Value: ", msg->detected);
 
  if(msg->detected == true) {
    m_qrCodeDetectedSomeTime = true;
  }
 
  if(msg->detected == false && m_qrCodeDetectedSomeTime) {
    m_autopilot.m_decision.start_driving();
  }
}
 
void AutoPilotNode::on_stop_line_callback(StopLineType::SharedPtr msg) {
  log_debug("AutoPilotNode: StopLine Message received. Value: ", msg->stopline_distance);
  static galaxis::lina::Vector2f positionOffset(0.f,0.f);
 
  if(msg->stopline_flag == true) {
    // additional check for orientation
    float min = 3.f;
    float max = 4.f;
 
    auto estimatedPositionMM = m_autopilot.m_localizer.get_latest_position_timepoint()->position - positionOffset;
    float currentOrientation = m_autopilot.m_localizer.get_latest_position_timepoint()->orientation;
 
    if(currentOrientation < min || currentOrientation > max) {
      m_autopilot.m_decision.update_stopline(msg->stopline_distance);
    }
  }
 
  if(msg->startline_flag == true) {
    // additional check for orientation
    float minOrientationForParking = 2.f;
    float maxOrientationForParking = 4.f;
 
    galaxis::lina::Vector2f currentPosition = m_autopilot.m_localizer.get_latest_position_timepoint()->position;
    float currentOrientation = m_autopilot.m_localizer.get_latest_position_timepoint()->orientation;
    if(currentOrientation >= minOrientationForParking && currentOrientation <= maxOrientationForParking) {
      positionOffset = currentPosition;
      m_autopilot.m_decision.update_starttargetline(0.8f);
    }
  }
}
 
void AutoPilotNode::on_parking_state_callback(ParkingStateType::SharedPtr msg) {
  log_debug("AutoPilotNode: ParkingState Message received. Value: ", int(msg->state));
 
  switch (msg->state) {
    case 1:
      m_autopilot.m_decision.set_parking_state(ParkingState::Idle);
      break;
    case 2:
      m_autopilot.m_decision.set_parking_state(ParkingState::Searching);
      break;
    case 3:
      m_autopilot.m_decision.set_parking_state(ParkingState::InParking); // parking in
      break;
    case 4:
      m_autopilot.m_decision.set_parking_state(ParkingState::InParking); // parking out
      break;
    case 5:
      m_autopilot.m_decision.set_parking_state(ParkingState::Done);
      break;
    default:
      break;
  }
 
  m_lastParkingState = msg->state;
}
 
void AutoPilotNode::send_map_debug_image() {
  if(m_mapDataImageDebugPub.getNumSubscribers() == 0) {
    log("AutoPilotNode [send_map_debug_image]: Skip sending message. Reason: NO Subscriber");
    return;
  }
  if(m_autopilot.m_laneCentersPairVecUPtr == nullptr || m_autopilot.m_linesObject == nullptr) {
    log_warning("AutoPilotNode [send_map_debug_image]: Cannot send map data (not existing). Skipped.");
    return;
  }
 
  std::shared_ptr<PositionTimepoint> positionTimepointNow = m_autopilot.m_localizer.get_latest_position_timepoint();
 
  std::shared_ptr<sensor_msgs::msg::Image> lineMessageUPtr = std::make_unique<sensor_msgs::msg::Image>();
 
  lineMessageUPtr->header.stamp = positionTimepointNow->timestamp;
 
  lineMessageUPtr->width = m_autopilot.m_mapDataLayer.m_lookupWindowSizeCMWidth; // m_autopilot.m_linesObject->windowSize[0];
  lineMessageUPtr->height = m_autopilot.m_mapDataLayer.m_lookupWindowSizeCMHeight + galaxis::autopilot::lookupLookBehindCarCM; // m_autopilot.m_linesObject->windowSize[1];
  lineMessageUPtr->encoding = sensor_msgs::image_encodings::RGB8;
  lineMessageUPtr->is_bigendian = 0;
  lineMessageUPtr->step = lineMessageUPtr->width * 3;
 
  lineMessageUPtr->data.clear();
  lineMessageUPtr->data.resize(lineMessageUPtr->step * lineMessageUPtr->height);
 
  int linePointsAmountOnImage = 0;
  
  // extract left line points
  LinePoints<int16_t>* lineMapObjPtr = &m_autopilot.m_linesObject->leftLine;
  int pixelByteIndex = galaxis::autopilot::imageLLinePixelByteIndex;
 
  Vector2f shiftVecForLookBehind = Vector2f(0.f, galaxis::autopilot::lookupLookBehindCarCM);
 
  for (int i = 0; i < 3; ++i) { // for all 3 lines
    for(auto& [pointXRaw, pointsYVec] : lineMapObjPtr->getPointsXY()) {
      for (auto& pointYRaw : pointsYVec) {
        // rotate point
        Vector2f vec = Vector2f(pointXRaw, pointYRaw);
        vec = m_autopilot.m_linesObject->reverseRotationMatrix * vec;
        // shift for look behind
        vec += shiftVecForLookBehind;
        
        // to integer
        int pointX = std::round(vec[0]) + lineMessageUPtr->width / 2;
        int pointY = lineMessageUPtr->height - std::round(vec[1]);
 
        //skip if not on image
        if (pointX < 0 || pointY < 0 || pointX >= static_cast<int>(lineMessageUPtr->width) || pointY >= static_cast<int>(lineMessageUPtr->height)) {
          continue;
        }
 
        try {
          int index = lineMessageUPtr->step * pointY + pointX * 3 + pixelByteIndex;
          if(index < 0 || index >= static_cast<int>(lineMessageUPtr->data.size())) continue;
 
          lineMessageUPtr->data[index] = 255;
 
          linePointsAmountOnImage++;
        }
        catch (const std::exception& ex) {
          log_warning("AutopilotNode: [send_map_debug_image] Exception thrown: ", ex.what());
        }
      }
    }
    // switch to next line (l->c->r)
    if (pixelByteIndex == galaxis::autopilot::imageLLinePixelByteIndex) {
      lineMapObjPtr = &m_autopilot.m_linesObject->centerLine;
      pixelByteIndex = galaxis::autopilot::imageCLinePixelByteIndex;
    }
    else if (pixelByteIndex == galaxis::autopilot::imageCLinePixelByteIndex) {
      lineMapObjPtr = &m_autopilot.m_linesObject->rightLine;
      pixelByteIndex = galaxis::autopilot::imageRLinePixelByteIndex;
    }
    else {
      // break;
    }
  }
 
  // get lane centers
  std::vector<galaxis::lina::Vector2f>* vectorPtr = &m_autopilot.m_laneCentersPairVecUPtr->second;
  bool rightLane = true;
  for (int i = 0; i < 2; ++i) { // for both lanes
    for(auto& point : *vectorPtr) {
      int imgPointX = std::round(point[0] + shiftVecForLookBehind[0]) + lineMessageUPtr->width / 2;
      int imgPointY = lineMessageUPtr->height - std::round(point[1] + shiftVecForLookBehind[1]);
 
      //skip if not on image
      if (imgPointX < 0 || imgPointY < 0 || imgPointX >= static_cast<int>(lineMessageUPtr->width) || imgPointY >= static_cast<int>(lineMessageUPtr->height)) {
        continue;
      }
 
      try {
        int index = lineMessageUPtr->step * imgPointY + imgPointX * 3;
        if(index < 0 || index + 2 >= static_cast<int>(lineMessageUPtr->data.size())) continue;
 
        if(rightLane) {
          lineMessageUPtr->data[index] = 255;
        }
        else {
          lineMessageUPtr->data[index + 1] = 255;
        }
 
        lineMessageUPtr->data[index + 2] = 255;
 
        linePointsAmountOnImage++;
      }
      catch (const std::exception& ex) {
        log_warning("AutopilotNode: [send_map_debug_image] Exception thrown: ", ex.what());
      }
    }
 
    vectorPtr = &m_autopilot.m_laneCentersPairVecUPtr->first;
    rightLane = false;
  }
 
  // draw grid for origin
  Vector2f origin = Vector2f(lineMessageUPtr->width / 2, galaxis::autopilot::lookupLookBehindCarCM);
  // X
  for(unsigned int i = 0; i < lineMessageUPtr->height; ++i) {
    int index = i * lineMessageUPtr->step + origin[0] * 3;
    if(index < 0 || index + 2 >= static_cast<int>(lineMessageUPtr->data.size())) log_warning("AutoPilotNode [send_map_debug_image]: While sending debug map message. (origin draw grid (X))");
    lineMessageUPtr->data[index] = 255;
    lineMessageUPtr->data[index + 1] = 255;
    lineMessageUPtr->data[index + 2] = 255;
  }
  // Y
  for(unsigned int i = 0; i < lineMessageUPtr->width; ++i) {
    int index = lineMessageUPtr->step * (lineMessageUPtr->height - galaxis::autopilot::lookupLookBehindCarCM) + 3*i;
    if(index < 0 || index + 2 >= static_cast<int>(lineMessageUPtr->data.size())) log_warning("AutoPilotNode [send_map_debug_image]: While sending debug map message. (origin draw grid (Y))");
    lineMessageUPtr->data[index] = 255;
    lineMessageUPtr->data[index + 1] = 255;
    lineMessageUPtr->data[index + 2] = 255;
  }
 
  // draws static obstacles
  std::shared_ptr<std::vector<StaticObstacle>> obstaclesVecSPtr = m_autopilot.m_mapDataLayer.getObstacles(positionTimepointNow->timestamp, false); // TODO clock
  Eigen::Rotation2Df rotationMat(positionTimepointNow->orientation);
  for(StaticObstacle const& obstacle : *obstaclesVecSPtr) {
    Vector2f relObsPos = obstacle.get_position_now() - positionTimepointNow->position / 10;
    relObsPos = rotationMat * relObsPos;
    relObsPos += shiftVecForLookBehind;
 
    int radiusCM = obstacle.get_width() / 2;
    for(int y = -radiusCM; y <= radiusCM; y++) {
      for(int x = -radiusCM; x <= radiusCM; x++) {
        if(x*x+y*y <= radiusCM*radiusCM) {
          int imageX = relObsPos[0] + x + lineMessageUPtr->width / 2;
          int imageY = lineMessageUPtr->height - relObsPos[1] + y;
          int index = imageY * lineMessageUPtr->step + imageX * 3;
 
          //skip if not on image
          if (imageX < 0 || imageY < 0 || imageX >= static_cast<int>(lineMessageUPtr->width) || imageY >= static_cast<int>(lineMessageUPtr->height)) {
            continue;
          }   
 
          if(index >= 0 && index + 2 <= static_cast<int>(lineMessageUPtr->data.size())) {
            lineMessageUPtr->data[index] = 255;
            lineMessageUPtr->data[index + 1] = 255;
            lineMessageUPtr->data[index + 2] = 255;
          }
        }
      }
    }
  }
 
  // draw left and right line polynom
  // left
  std::vector<Vector2f>& points = *m_autopilot.m_leftLinePolynomPointsVecSPtr;
  uint8_t r = 249;
  uint8_t g = 215;
  uint8_t b = 28;
 
  for(int i = 0; i < 2; i++) {
    if (i == 1) {
      auto& points = m_autopilot.m_rightLinePolynomPointsVecSPtr;
      r = 255;
      g = 165;
      b = 0;
    }
  for(auto const& point : points) {
    int imgPointX = std::round(point[0] + shiftVecForLookBehind[0]) + lineMessageUPtr->width / 2;
    int imgPointY = lineMessageUPtr->height - std::round(point[1] + shiftVecForLookBehind[1]);
    
    //skip if not on image
    if (imgPointX < 0 || imgPointY < 0 || imgPointX >= static_cast<int>(lineMessageUPtr->width) || imgPointY >= static_cast<int>(lineMessageUPtr->height)) {
      continue;
    }
 
    try {
      int index = lineMessageUPtr->step * imgPointY + imgPointX * 3;
      if(index < 0 || index + 2 >= static_cast<int>(lineMessageUPtr->data.size())) continue;
 
      if(rightLane) {
        lineMessageUPtr->data[index] = r;
      }
      else {
        lineMessageUPtr->data[index + 1] = g;
      }
 
      lineMessageUPtr->data[index + 2] = b;
 
      linePointsAmountOnImage++;
    }
    catch (const std::exception& ex) {
      log_warning("AutopilotNode: [send_map_debug_image] Exception thrown: ", ex.what());
    }
  }
  }
 
  // draw trajectory polynom
  if(m_currentDriveInformationSPtr != nullptr) {
    for(int x = -shiftVecForLookBehind[1]; x < lineMessageUPtr->height - shiftVecForLookBehind[1]; ++x) {
      // evaluate polynom at x
      int y = m_autopilot.eval_poly(m_currentDriveInformationSPtr->polynom, static_cast<float>(x) * 0.01) * 100; // m to cm
 
      // point must be rotated by 90 cc
      int imgPointX = std::round(-y + shiftVecForLookBehind[0]) + lineMessageUPtr->width / 2;
      int imgPointY = lineMessageUPtr->height - std::round(x + shiftVecForLookBehind[1]);
      
      //skip if not on image
      if (imgPointX < 0 || imgPointY < 0 || imgPointX >= static_cast<int>(lineMessageUPtr->width) || imgPointY >= static_cast<int>(lineMessageUPtr->height)) {
        continue;
      }
 
      try {
        int index = lineMessageUPtr->step * imgPointY + imgPointX * 3;
        if(index < 0 || index + 2 >= static_cast<int>(lineMessageUPtr->data.size())) continue;
 
        lineMessageUPtr->data[index]   = 204;
        lineMessageUPtr->data[index+1] = 204;
        lineMessageUPtr->data[index+2] = 204;
 
        linePointsAmountOnImage++;
      }
      catch (const std::exception& ex) {
        log_warning("AutopilotNode: [send_map_debug_image] Exception thrown: ", ex.what());
      }
    }
  }
 
  // publish message
  m_mapDataImageDebugPub.publish(lineMessageUPtr);
 
  log("AutoPilotNode [send_map_debug_image]: Data Layer Image Message sent. LinePointsAmountOnImage: ", linePointsAmountOnImage);
}
 
void AutoPilotNode::send_full_map_debug_image() {
  if(m_fullMapDataImageDebugPub.getNumSubscribers() == 0) {
    log("AutoPilotNode [send_full_map_debug_image]: Skip sending message. Reason: NO Subscriber");
    return;
  }
 
  MicroStopwatch stopwatch;
 
  Vector2i extractionWindow = Vector2i(1400, 1200);
  
  auto lineMessageUPtr = std::make_unique<sensor_msgs::msg::Image>();
  
  lineMessageUPtr->header.stamp = this->get_clock()->now();
  lineMessageUPtr->width = extractionWindow[0];
  lineMessageUPtr->height = extractionWindow[1];
  lineMessageUPtr->encoding = sensor_msgs::image_encodings::RGB8;
  lineMessageUPtr->is_bigendian = 0;
  lineMessageUPtr->step = lineMessageUPtr->width * 3;
 
  lineMessageUPtr->data.clear();
  lineMessageUPtr->data.resize(lineMessageUPtr->step * lineMessageUPtr->height);
 
 
  auto linesObjRaw = m_autopilot.m_mapDataLayer.getLineObject(Vector2f(-700.f, -600.f), 0, 0, extractionWindow);
 
  // extract left line points
  LinePoints<int16_t>* lineObjPtr = &linesObjRaw->leftLine;
  int pixelByteIndex = galaxis::autopilot::imageLLinePixelByteIndex;
 
  for (int i = 0; i < 3; ++i) { // for all 3 lines
    for(auto& [pointXRaw, pointsYVec] : lineObjPtr->getPointsXY()) {
      for (auto& pointYRaw : pointsYVec) {
        // rotate point
        Vector2f vec = Vector2f(pointXRaw, pointYRaw);
        // vec = linesObjRaw->reverseRotationMatrix * vec; => no need because no rotation
        
        // to integer
        int pointX = std::round(vec[0]) + lineMessageUPtr->width / 2;
        int pointY = lineMessageUPtr->height - std::round(vec[1]);
 
        //skip if out of image
        if (pointX < 0 || pointY < 0 || pointX >= static_cast<int>(lineMessageUPtr->width) || pointY >= static_cast<int>(lineMessageUPtr->height)) {
          continue;
        }
 
        try {
          int index = lineMessageUPtr->step * pointY + pointX * 3 + pixelByteIndex;
          if(index >= static_cast<int>(lineMessageUPtr->data.size())) continue;
 
          lineMessageUPtr->data[index] = 255;
        }
        catch (const std::exception& ex) {
          log_warning("AutoPilotNode: [send_full_map_debug_image] Exception thrown: ", ex.what());
        }
      }
    }
    // switch to next line (l->c->r)
    if (pixelByteIndex == galaxis::autopilot::imageLLinePixelByteIndex) {
      lineObjPtr = &linesObjRaw->centerLine;
      pixelByteIndex = galaxis::autopilot::imageCLinePixelByteIndex;
    }
    else if (pixelByteIndex == galaxis::autopilot::imageCLinePixelByteIndex) {
      lineObjPtr = &linesObjRaw->rightLine;
      pixelByteIndex = galaxis::autopilot::imageRLinePixelByteIndex;
    }
    else {
      // break;
    }
 
  }
 
  log("AutoPilotNode [send_full_map_debug_image]: Data Layer FULL MAP Image Message sent. Time consumed [ms]: ", stopwatch.get_milliseconds());
 
  m_fullMapDataImageDebugPub.publish(std::move(lineMessageUPtr));
}
 
rcl_interfaces::msg::SetParametersResult AutoPilotNode::new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
  log("[AutopilotNode] NEW PARAMETERS");
  
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  result.reason = "";
 
  for (const auto &param: parameters) {
    if (param.get_name() == "obstacle.position_diff_for_rerecognition_cm") {
      if(check_parameter<int>(param, result, 0)) {
        m_autopilot.m_mapDataLayer.setObstaclePositionDiffForRerecognition(param.as_int());
      }
    }
    else if (param.get_name() == "obstacle.obstacle_lookup_distance_cm") {
      if(check_parameter<int>(param, result, 0)) {
        m_autopilot.set_obstacle_lookup_distance(param.as_int());
      }
    }
    else if (param.get_name() == "obstacle.obstacle_lookup_distance_step_cm") {
      if(check_parameter<int>(param, result, 0)) {
        m_autopilot.set_obstacle_lookup_distance_step(param.as_int());
      }
    }
    else if (param.get_name() == "obstacle.obstacle_to_lane_center_minimum_distance_for_dodge_cm") {
      if(check_parameter<int>(param, result, 0)) {
        m_autopilot.set_obstacle_to_lane_center_min_distance_for_dodge(param.as_int());
      }
    }
    else if (param.get_name() == "obstacle.obstacle_track_time_ms") {
      if(check_parameter<int>(param, result, 0)) {
        div_t divres = div(int(param.as_int()), 1000);
        m_autopilot.m_mapDataLayer.setObstacleTrackTimeDuration(divres.quot, divres.rem * 1e6);
      }
    }
    else if (param.get_name() == "map_data_layer.lookup_window_size_width_cm") {
      if(check_parameter<int>(param, result, 0, 300.0)) {
        m_autopilot.m_mapDataLayer.setLookupWindowSizeCMWidth(param.as_int());
      }
    }
    else if (param.get_name() == "map_data_layer.lookup_window_size_height_cm") {
      if(check_parameter<int>(param, result, 0, 300.0)) {
        m_autopilot.m_mapDataLayer.setLookupWindowSizeCMHeight(param.as_int());
      }
    }
    // else if (param.get_name() == "map_data_layer.lookup_look_behind_car_cm") {
    //   if(check_parameter<int>(param, result, 0, 100.0)) {
    //     m_autopilot.m_mapDataLayer.setLookupLookBehindCarCM(param.as_int());
    //   }
    // }
    else if (param.get_name() == "map_data_layer.grid_cell_threshold_import") {
      if(check_parameter<int>(param, result, 0, 255)) {
        m_autopilot.m_mapDataLayer.setCellThresholdRGBValue(param.as_int());
      }
    }
    else if (param.get_name() == "map_data_layer.grid_cell_threshold_extract") {
      if(check_parameter<int>(param, result, 0, 255)) {
        m_autopilot.m_mapDataLayer.setCellThresholdExtract(param.as_int());
      }
    }
    else if (param.get_name() == "map_data_layer.grid_cell_threshold_decrease_per_step") {
      if(check_parameter<int>(param, result, 0, 255)) {
        m_autopilot.m_mapDataLayer.setCellLineDecreasePerView(param.as_int());
      }
    }
    else if (param.get_name() == "map_data_layer.grid_cell_threshold_increase_per_view") {
      if(check_parameter<int>(param, result, 0, 255)) {
        m_autopilot.m_mapDataLayer.setCellLineIncreasePerView(param.as_int());
      }
    }
    else if (param.get_name() == "map_data_extraction_postprocessing.cluster_line_eps_cm") {
      if(check_parameter<int>(param, result, 1, 50)) {
        m_autopilot.set_line_cluster_eps(param.as_int());
      }
    }
    else if (param.get_name() == "map_data_extraction_postprocessing.cluster_line_min_pts") {
      if(check_parameter<int>(param, result, 1, 50)) {
        m_autopilot.set_line_cluster_min_pts(param.as_int());
      }
    }
    else if (param.get_name() == "trajectory_polynom.degree") {
      if(check_parameter<int>(param, result, 1, 10)) {
        m_autopilot.set_polynom_degree(param.as_int());
      }
    }
    else if (param.get_name() == "trajectory_polynom.minimum_points_amount") {
      if(check_parameter<int>(param, result)) {
        m_autopilot.set_polynom_min_points_amount(param.as_int());
      }
    }
    else if (param.get_name() == "trajectory_polynom.append_points_behind_amount") {
      if(check_parameter<int>(param, result, 0)) {
        m_autopilot.set_traj_points_to_append_amount(param.as_int());
      }
    }
    else if (param.get_name() == "trajectory_polynom.append_points_behind_distance_cm") {
      if(check_parameter<int>(param, result, 1, 200)) {
        m_autopilot.set_traj_points_start_appending_offset(param.as_int());
      }
    }
    else if (param.get_name() == "decision.velocity_reference_m_s") {
      if(check_parameter<double>(param, result, 0.0, 3.0)) {
        m_autopilot.m_decision.set_velocity_reference(param.as_double());
      }
    }
    else if (param.get_name() == "decision.distance_after_switch_back_to_drive_lane_m") {
      if(check_parameter<double>(param, result, 0.0, 10.0)) {
        m_autopilot.m_decision.set_distance_after_switch_back_lane(param.as_double());
      }
    }
    else if (param.get_name() == "decision.critical_obstacle_distance_for_lane_changing_cm") {
      if(check_parameter<int>(param, result, 0, 500)) {
        m_autopilot.m_decision.set_critical_obstacle_distance_for_lane_changing(param.as_int());
      }
    }
    else if (param.get_name() == "decision.velocity_reference_parking_zone_m_s") {
      if(check_parameter<double>(param, result, 0, 2)) {
        m_autopilot.m_decision.set_velocity_reference_parking(param.as_double());
      }
    }
    else if (param.get_name() == "controller_reference.send_sleep_time_micros") {
      if(check_parameter<int>(param, result, 1, 100000)) {
        m_controllerReferenceSendSleepTimeMicroSec = param.as_int();
      }
    }
    else if (param.get_name() == "lane_center_improvement.polynom_evaluation_step_cm") {
      if(check_parameter<int>(param, result, 1)) {
        m_autopilot.set_line_polynom_evaluation_step(param.as_int());
      }
    }
    else if (param.get_name() == "lane_center_improvement.polynom_evaluation_step_size_cm") {
      if(check_parameter<int>(param, result, 1, 100)) {
        m_autopilot.set_line_polynom_evaluation_step_size(param.as_int());
      }
    }
    else if (param.get_name() == "lane_center_improvement.minimum_number_of_clustered_line_points_to_activate") {
      if(check_parameter<int>(param, result, 0)) {
        m_autopilot.set_min_number_of_clustered_line_points(param.as_int());
      }
    }
    else if (param.get_name() == "control_lines.ignore_stopline_in_start_area_radius_mm") {
      if(check_parameter<double>(param, result, 0, 100000)) {
        m_stoplineIgnoreAreaRadius = param.as_double();
      }
    }
    else {
      result.successful = false;
      result.reason += "[" + param.get_name() + "] UNKNOWN param or NOT IMPLEMENTED; ";
    }
  }
 
  if(result.successful == false) {
    log_warning("[AutopilotNode] Failed to set some parameters. Reasons: ", result.reason);
  }
 
  return result;
}