Galaxis Codebase: AutoPilotNode Class Reference

#include <AutoPilotNode.hpp>

Inheritance diagram for AutoPilotNode:

Collaboration diagram for AutoPilotNode:

[legend]

Public Member Functions
	AutoPilotNode ()

	~AutoPilotNode ()

Private Member Functions
void	on_system_settings_callback (SystemSettingsType::SharedPtr msg)

void	car_pose_publish_loop ()

void	debug_publish_loop ()

void	controller_reference_publish_loop ()

void	lane_detection_shared_memory_loop ()

void	on_simulation_pose_callback (geometry_msgs::msg::Pose::SharedPtr p_message)

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 ()

void	send_map_debug_image ()

void	send_full_map_debug_image ()

rcl_interfaces::msg::SetParametersResult	new_parameters_callback (const std::vector< rclcpp::Parameter > &parameters)

Private Attributes
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

int	m_poseSendSleepTimeMicroSec = 10000

std::atomic< uint8_t >	m_lastParkingState

rclcpp::Time	m_lastLaneDetectionUpdate

AutoPilot	m_autopilot

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

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::Publisher	m_mapDataImageDebugPub

image_transport::Publisher	m_laneDetectionMessage

image_transport::Publisher	m_fullMapDataImageDebugPub

std::shared_ptr< DriveInformation_t >	m_currentDriveInformationSPtr = nullptr

std::mutex	m_controllerPublishMtx

com::SharedMemory	m_laneDetectionSharedMemory

size_t	m_laneDetectionImageSize = 256 * 256 * 3

bool	m_qrCodeDetectedSomeTime = false

std::atomic< float >	m_stoplineIgnoreAreaRadius

Additional Inherited Members
Protected Member Functions inherited from GalaxisNode
	GalaxisNode ()=delete

	GalaxisNode (std::string p_name, unsigned int p_heartbeatTimePeriod)

	GalaxisNode (std::string p_name)

template<typename MsgSPtr >
void	tickSub (std::map< galaxis::serviceStates::State, std::function< void(const MsgSPtr)>> const *p_functionMapPtr, MsgSPtr const p_message)

template<typename... Args>
void	tickPub (std::map< galaxis::serviceStates::State, std::function< void(Args...)>> const *p_functionMapPtr, Args... p_args)

void	register_callback (std::function< void(galaxis::serviceStates::State)> p_function)

void	service_state_callback (ServiceStateType::SharedPtr const p_msgSPtr)

galaxis::serviceStates::State	get_current_service_state () const

void	enable_parameters (std::function< rcl_interfaces::msg::SetParametersResult(const std::vector< rclcpp::Parameter > &)> callbackFunc)

void	create_parameters (std::shared_ptr< json > configJsonSPtr)

json const &	get_global_settings ()

Detailed Description

Definition at line 23 of file AutoPilotNode.hpp.

Constructor & Destructor Documentation

◆ AutoPilotNode()

AutoPilotNode::AutoPilotNode ( )

Definition at line 18 of file AutoPilotNode.cpp.

                              : 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
 }

References Localizer::add_position_timepoint(), com::SharedMemory::allocate(), car_pose_publish_loop(), controller_reference_publish_loop(), debug_publish_loop(), GalaxisNode::enable_parameters(), MapDataLayer::enableSimulationMode(), GalaxisNode::get_global_settings(), lane_detection_shared_memory_loop(), galaxis::utils::log(), galaxis::utils::log_warning(), m_autopilot, m_autopilotStatePubSPtr, m_carposePubSPtr, m_controllerRefPubSPtr, m_debugMode, AutoPilot::m_decision, m_fakeLineDetectionMode, m_fullMapDataImageDebugPub, m_laneDetectionImageSize, m_laneDetectionMessage, m_laneDetectionSharedMemory, m_laneDetectionSharedMemoryMode, m_lastLaneDetectionUpdate, m_lastParkingState, AutoPilot::m_localizer, m_mapDataImageDebugPub, AutoPilot::m_mapDataLayer, m_measurementSub, m_parkingOperationPubSPtr, m_pathpolynomPubSPtr, m_poseSendSleepTimeMicroSec, m_sendFullMapDebugMsg, m_sendLaneDetectionImage, m_simulationMode, Decision::m_stopAtStartLine, m_stoplineIgnoreAreaRadius, m_subscriberCarPoseSPtr, m_subscriberLaneDetectionSub, m_subscriberObstaclesSPtr, m_subscriberParkingStateSPtr, m_subscriberQRCodeSPtr, m_subscriberStopLine, new_parameters_callback(), on_ecu_measurement_callback(), on_obstacle_callback(), on_parking_state_callback(), on_qr_code_callback(), on_simulation_pose_callback(), on_stop_line_callback(), on_vci_lane_detection_callback(), galaxis::utils::read_json_file(), galaxis::autopilot::realPixelToPositionConversionFactorCM, MapDataLayer::setLaneImageConversionFactor(), galaxis::autopilot::simulationPixelToPositionConversionFactorCM, and Decision::start_driving().

Here is the call graph for this function:

◆ ~AutoPilotNode()

AutoPilotNode::~AutoPilotNode ( )

Definition at line 150 of file AutoPilotNode.cpp.

150 {}

Member Function Documentation

◆ car_pose_publish_loop()

void AutoPilotNode::car_pose_publish_loop ( )

private

Definition at line 152 of file AutoPilotNode.cpp.

                                           {
   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));
   }
 }

References Localizer::get_latest_position_timepoint(), Localizer::is_empty(), m_autopilot, m_carposePubSPtr, AutoPilot::m_localizer, and m_poseSendSleepTimeMicroSec.

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ controller_reference_publish_loop()

void AutoPilotNode::controller_reference_publish_loop ( )

private

Definition at line 169 of file AutoPilotNode.cpp.

                                                       {
   while(1) {
     MicroStopwatch stopwatch;
     send_controller_references();
     long long time = stopwatch.get_time();
     time = m_controllerReferenceSendSleepTimeMicroSec - time;
     if(time > 0) usleep(time);
   }
 }

References galaxis::utils::MicroStopwatch::get_time(), m_controllerReferenceSendSleepTimeMicroSec, and send_controller_references().

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ debug_publish_loop()

void AutoPilotNode::debug_publish_loop ( )

private

Definition at line 179 of file AutoPilotNode.cpp.

                                        {
   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);
     }
   }
 }

References Decision::get_current_situationstate(), galaxis::utils::MicroStopwatch::get_time(), galaxis::utils::log(), m_autopilot, m_autopilotStatePubSPtr, m_controllerPublishMtx, AutoPilot::m_decision, m_sendFullMapDebugMsg, send_full_map_debug_image(), and send_map_debug_image().

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ lane_detection_shared_memory_loop()

void AutoPilotNode::lane_detection_shared_memory_loop ( )

private

Definition at line 202 of file AutoPilotNode.cpp.

                                                       {
   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);
     }
   }
 }

References com::SharedData::data, com::SharedData::dataSize, com::SharedMemory::is_new_data_available(), galaxis::utils::log(), galaxis::utils::log_warning(), m_laneDetectionImageSize, m_laneDetectionMessage, m_laneDetectionSharedMemory, m_sendLaneDetectionImage, on_vci_lane_detection_callback(), com::SharedMemory::read_mutexed(), com::SharedData::tv_nsec, and com::SharedData::tv_sec.

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ new_parameters_callback()

rcl_interfaces::msg::SetParametersResult AutoPilotNode::new_parameters_callback ( const std::vector< rclcpp::Parameter > & parameters )

private

Definition at line 812 of file AutoPilotNode.cpp.

                                                                                                                         {
   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;
 }

References galaxis::utils::log(), galaxis::utils::log_warning(), m_autopilot, m_controllerReferenceSendSleepTimeMicroSec, AutoPilot::m_decision, AutoPilot::m_mapDataLayer, m_stoplineIgnoreAreaRadius, Decision::set_critical_obstacle_distance_for_lane_changing(), Decision::set_distance_after_switch_back_lane(), AutoPilot::set_line_cluster_eps(), AutoPilot::set_line_cluster_min_pts(), AutoPilot::set_line_polynom_evaluation_step(), AutoPilot::set_line_polynom_evaluation_step_size(), AutoPilot::set_min_number_of_clustered_line_points(), AutoPilot::set_obstacle_lookup_distance(), AutoPilot::set_obstacle_lookup_distance_step(), AutoPilot::set_obstacle_to_lane_center_min_distance_for_dodge(), AutoPilot::set_polynom_degree(), AutoPilot::set_polynom_min_points_amount(), AutoPilot::set_traj_points_start_appending_offset(), AutoPilot::set_traj_points_to_append_amount(), Decision::set_velocity_reference(), Decision::set_velocity_reference_parking(), MapDataLayer::setCellLineDecreasePerView(), MapDataLayer::setCellLineIncreasePerView(), MapDataLayer::setCellThresholdExtract(), MapDataLayer::setCellThresholdRGBValue(), MapDataLayer::setLookupWindowSizeCMHeight(), MapDataLayer::setLookupWindowSizeCMWidth(), MapDataLayer::setObstaclePositionDiffForRerecognition(), and MapDataLayer::setObstacleTrackTimeDuration().

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_ecu_measurement_callback()

void AutoPilotNode::on_ecu_measurement_callback ( MeasurementsType::SharedPtr msg )

private

Definition at line 270 of file AutoPilotNode.cpp.

                                                                              {
   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*/);
 }

References Localizer::add_state(), galaxis::motorSettings::erpmToVelocity(), galaxis::utils::log(), m_autopilot, AutoPilot::m_decision, AutoPilot::m_localizer, and Decision::update_tof().

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_obstacle_callback()

void AutoPilotNode::on_obstacle_callback ( ObstacleListType::SharedPtr msg )

private

Definition at line 402 of file AutoPilotNode.cpp.

                                                                       {
   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);
   }
 }

References MapDataLayer::addOrUpdateStaticObstacle(), Localizer::get_position_timepoint_by_time(), Localizer::is_empty(), galaxis::utils::log(), galaxis::utils::log_warning(), m_autopilot, AutoPilot::m_localizer, and AutoPilot::m_mapDataLayer.

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_parking_state_callback()

void AutoPilotNode::on_parking_state_callback ( ParkingStateType::SharedPtr msg )

private

Definition at line 464 of file AutoPilotNode.cpp.

                                                                            {
   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;
 }

References Done, Idle, InParking, galaxis::utils::log_debug(), m_autopilot, AutoPilot::m_decision, m_lastParkingState, Searching, and Decision::set_parking_state().

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_qr_code_callback()

void AutoPilotNode::on_qr_code_callback ( QRCodeDetectType::SharedPtr msg )

private

Definition at line 421 of file AutoPilotNode.cpp.

                                                                      {
   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();
   }
 }

References galaxis::utils::log_debug(), m_autopilot, AutoPilot::m_decision, m_qrCodeDetectedSomeTime, and Decision::start_driving().

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_simulation_pose_callback()

void AutoPilotNode::on_simulation_pose_callback ( geometry_msgs::msg::Pose::SharedPtr p_message )

private

Definition at line 240 of file AutoPilotNode.cpp.

                                                                                        {
   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());
 }

References Localizer::add_position_timepoint(), Decision::get_velocity_reference(), m_autopilot, AutoPilot::m_decision, AutoPilot::m_localizer, MATH_PI_DOUBLE, and galaxis::autopilot::simluationCarPositionOffsetToFrontAxisM.

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_stop_line_callback()

void AutoPilotNode::on_stop_line_callback ( StopLineType::SharedPtr msg )

private

Definition at line 433 of file AutoPilotNode.cpp.

                                                                    {
   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);
     }
   }
 }

References Localizer::get_latest_position_timepoint(), galaxis::utils::log_debug(), m_autopilot, AutoPilot::m_decision, AutoPilot::m_localizer, Decision::update_starttargetline(), and Decision::update_stopline().

Referenced by AutoPilotNode().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ on_system_settings_callback()

void AutoPilotNode::on_system_settings_callback ( SystemSettingsType::SharedPtr msg )

private

◆ on_vci_lane_detection_callback()

void AutoPilotNode::on_vci_lane_detection_callback ( sensor_msgs::msg::Image::SharedPtr msg )

private

Definition at line 282 of file AutoPilotNode.cpp.

                                                                                    {
   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();
 }

References Localizer::get_position_timepoint_by_time(), galaxis::utils::MicroStopwatch::get_time(), Localizer::is_empty(), galaxis::utils::log(), galaxis::utils::log_debug(), galaxis::utils::log_error(), galaxis::utils::log_warning(), m_autopilot, m_debugMode, m_lastLaneDetectionUpdate, AutoPilot::m_localizer, AutoPilot::m_mapDataLayer, and MapDataLayer::updateMap().

Referenced by AutoPilotNode(), and lane_detection_shared_memory_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_controller_references()

void AutoPilotNode::send_controller_references ( )

private

Definition at line 324 of file AutoPilotNode.cpp.

                                                {
   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));
 }

References AutoPilot::get_drive_information(), galaxis::utils::MicroStopwatch::get_time(), galaxis::utils::log(), galaxis::utils::log_error(), m_autopilot, m_controllerPublishMtx, m_controllerRefPubSPtr, m_currentDriveInformationSPtr, m_parkingOperationPubSPtr, and m_pathpolynomPubSPtr.

Referenced by controller_reference_publish_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_full_map_debug_image()

void AutoPilotNode::send_full_map_debug_image ( )

private

Definition at line 736 of file AutoPilotNode.cpp.

                                               {
   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));
 }

References galaxis::utils::MicroStopwatch::get_milliseconds(), MapDataLayer::getLineObject(), map::classes::LinePoints< T >::getPointsXY(), galaxis::autopilot::imageCLinePixelByteIndex, galaxis::autopilot::imageLLinePixelByteIndex, galaxis::autopilot::imageRLinePixelByteIndex, galaxis::utils::log(), galaxis::utils::log_warning(), m_autopilot, m_fullMapDataImageDebugPub, and AutoPilot::m_mapDataLayer.

Referenced by debug_publish_loop().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_map_debug_image()

void AutoPilotNode::send_map_debug_image ( )

private

Definition at line 490 of file AutoPilotNode.cpp.

                                          {
   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);
 }