galaxis::utils Namespace Reference

Classes
class	MicroStopwatch

Functions
long long	get_time_now_ms ()
long long	get_time_now_us ()
shared_ptr< vector< string > >	read_from_file_to_string (string const &p_filepath)
shared_ptr< vector< float > >	read_from_file_to_float (string const &p_filepath)
template<typename T >
void	write_to_file (string const &p_filepath, std::vector< std::vector< T >> const &p_data, string const &p_delimiter=",")
shared_ptr< std::map< std::string, std::string > >	read_configuration (string const &p_filepath)
template<typename... Args>
void	log_nonewline (const Args &... args)
template<typename... Args>
void	log_cout (const Args &... args)
template<typename... Args>
void	log (const Args &... args)
template<typename... Args>
void	log_warning (const Args &... args)
template<typename... Args>
void	log_error (const Args &... args)
template<typename... Args>
void	log_debug (const Args &... args)
float	evaluate_polynom (polynom const &p_polynomRef, float p_x)
double	evaluate_polynom (std::vector< double > const &p_polynomRef, double p_x)
float	calculate_gradient (std::vector< float > const &p_polynomRef, float p_x)
double	calculate_gradient (std::vector< double > const &p_polynomRef, double p_x)
double	calculate_gradient (DynamicPolynom p_polynomVecSPtr, double p_x)
shared_ptr< json >	read_json_file (std::string path)
shared_ptr< json >	read_system_settings (std::string key)
shared_ptr< std::string >	read_file_to_string (std::string path)
template<typename T , typename = typename std::enable_if<std::is_arithmetic<T>::value, T>::type>
bool	check_parameter (rclcpp::Parameter const &param, rcl_interfaces::msg::SetParametersResult &result, T min=numeric_limits< T >::min(), T max=numeric_limits< T >::max())

Function Documentation

calculate_gradient() [1/3]

double galaxis::utils::calculate_gradient	(	DynamicPolynom	p_polynomVecSPtr,
		double	p_x
	)

Definition at line 96 of file poly.cpp.

                                                                                   {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 1; i < p_polynomVecSPtr->size(); ++i) {
    iValue = 1;
 
    // i*x^(i-1)
    for(size_t j = 0; j < i - 1; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomVecSPtr->at(i) * i * iValue;
  }
 
  return value;
}

calculate_gradient() [2/3]

double galaxis::utils::calculate_gradient	(	std::vector< double > const &	p_polynomRef,
		double	p_x
	)

Definition at line 78 of file poly.cpp.

                                                                                         {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 1; i < p_polynomRef.size(); ++i) {
    iValue = 1;
 
    // i*x^(i-1)
    for(size_t j = 0; j < i - 1; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomRef[i] * i * iValue;
  }
 
  return value;
}

calculate_gradient() [3/3]

float galaxis::utils::calculate_gradient	(	std::vector< float > const &	p_polynomRef,
		float	p_x
	)

Definition at line 60 of file poly.cpp.

                                                                                      {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 1; i < p_polynomRef.size(); ++i) {
    iValue = 1;
 
    // i*x^(i-1)
    for(size_t j = 0; j < i - 1; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomRef[i] * i * iValue;
  }
 
  return value;
}

Referenced by AutoPilot::get_drive_information(), and main().

Here is the caller graph for this function:

check_parameter()

template<typename T , typename = typename std::enable_if<std::is_arithmetic<T>::value, T>::type>

bool galaxis::utils::check_parameter	(	rclcpp::Parameter const &	param,
		rcl_interfaces::msg::SetParametersResult &	result,
		T	min = numeric_limits<T>::min(),
		T	max = numeric_limits<T>::max()
	)

Definition at line 29 of file settings.hpp.

        {
      
      T paramValue;
      if( std::is_same<T, int>::value ) {
        if( param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER ) {
          paramValue = param.as_int();
        }
        else {
          result.successful = false;
          result.reason += "[" + param.get_name() + "] Type wrong. Must be INTEGER; ";
          return false;
        }
      }
      else if ( std::is_same<T, double>::value || std::is_same<T, float>::value ) {
        if( param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE ) {
          paramValue = param.as_double();
        }
        else {
          result.successful = false;
          result.reason += "[" + param.get_name() + "] Type wrong. Must be DOUBLE; ";
          return false;
        }
      }
      else {
        result.successful = false;
        result.reason += "[" + param.get_name() + "] Type wrong.; ";
        return false;
      }
 
      if( paramValue < min ) {
        result.successful = false;
        result.reason += "[" + param.get_name() + "] Value too low. Must be larger or equal to " + std::to_string(min) + " ; ";
        return false;
      }
      if( paramValue > max ) {
        result.successful = false;
        result.reason += "[" + param.get_name() + "] Value too high. Must be lower or equal to " + std::to_string(max) + " ; ";
        return false;
      }
 
      return true;
    }

evaluate_polynom() [1/2]

float galaxis::utils::evaluate_polynom	(	polynom const &	p_polynomRef,
		float	p_x
	)

Definition at line 50 of file poly.cpp.

                                                                           {
  std::vector<double> poly;
  for(auto const& elt : p_polynomRef) {
    poly.push_back(elt);
  }
 
  return static_cast<float>(evaluate_polynom(poly,p_x));
}

Referenced by galaxis::PolySpline::evaluate(), and AutoPilot::get_lane_centers_via_polynom_thread_helper().

Here is the caller graph for this function:

evaluate_polynom() [2/2]

double galaxis::utils::evaluate_polynom	(	std::vector< double > const &	p_polynomRef,
		double	p_x
	)

Definition at line 32 of file poly.cpp.

                                                                                       {
  float value = 0.0;
 
  float iValue;
  for(size_t i = 0; i < p_polynomRef.size(); ++i) {
    iValue = 1;
 
    // x^i
    for(size_t j = 0; j < i; ++j) {
      iValue *= p_x;
    }
 
    value += p_polynomRef[i] * iValue;
  }
 
  return value;
}

get_time_now_ms()

long long galaxis::utils::get_time_now_ms

(

)

Definition at line 6 of file clock.cpp.

                                        {
  auto time_now    = std::chrono::system_clock::now();
  auto time_now_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(time_now);
  auto time_value  = time_now_ms.time_since_epoch();
  return time_value.count();
}

get_time_now_us()

long long galaxis::utils::get_time_now_us

(

)

Definition at line 13 of file clock.cpp.

                                        {
  auto time_now    = std::chrono::high_resolution_clock::now();
  auto time_now_ns = std::chrono::time_point_cast<std::chrono::microseconds>(time_now);
  auto time_value  = time_now_ns.time_since_epoch();
  return time_value.count();
}

Referenced by galaxis::utils::MicroStopwatch::get(), and galaxis::utils::MicroStopwatch::start().

Here is the caller graph for this function:

log()

template<typename... Args>

void galaxis::utils::log

(

const Args &...

args

)

Definition at line 33 of file logger.hpp.

                                  {
#ifdef _LOGGER_INFO
      (std::clog << ... << args);
      std::clog << "\n";
#endif
    }

Referenced by AutoPilotNode::AutoPilotNode(), galaxis::control::TwoAndAHalfCarrots::calculate_best_yaw_rate(), ControllerNode::calculate_turn_angle(), VCINodeAsync::camera_publish_loop(), LaneDetectionGT::checkReady(), ControllerNode::ControllerNode(), GalaxisNode::create_parameters(), AutoPilotNode::debug_publish_loop(), CameraFrameObserver::FrameReceived(), LaneDetectionGT::groundtruthCallback(), LaneDetectionGT::groundtruthObstacleCallback(), ECUNode::init(), Realsense::init(), MapDataLayer::init(), AutoPilotNode::lane_detection_shared_memory_loop(), LaneDetectionGT::LaneDetectionGT(), RemoteControl::loop(), LaneDetectionGT::loop(), masterNode::masterNode(), ECUNode::motion_callback(), StopLineDetection::new_parameters_callback(), RealSenseNode::new_parameters_callback(), ControllerNode::new_parameters_callback(), AutoPilotNode::new_parameters_callback(), RealSenseNode::object_detection(), ControllerNode::on_controller_reference_callback(), AutoPilotNode::on_ecu_measurement_callback(), ControllerNode::on_map_debug_image_callback(), ControllerNode::on_measurement_callback(), AutoPilotNode::on_obstacle_callback(), ControllerNode::on_pathpolynom_callback(), StopLineDetection::on_stop_line_detection_callback(), AutoPilotNode::on_vci_lane_detection_callback(), QRCodeDetectionNode::onImgCallback(), ControllerNode::publish_control_values(), ECUNode::publishMeasurements(), RemoteControl::RemoteControl(), AutoPilotNode::send_controller_references(), ControllerNode::send_debug_messages(), AutoPilotNode::send_full_map_debug_image(), AutoPilotNode::send_map_debug_image(), ECUNode::settingsvalues_callback(), SimulationAdapterNode::SimulationAdapterNode(), SimulationAdapterNode::simulationUpdateLoop(), PolyFit< T >::solveRLS(), StopLineDetection::StopLineDetection(), SimulationAdapterNode::updatePose(), SimulationAdapterNode::vehicleControlCallback(), and masterNode::~masterNode().

Here is the caller graph for this function:

log_cout()

template<typename... Args>

void galaxis::utils::log_cout

(

const Args &...

args

)

Definition at line 25 of file logger.hpp.

                                       {
#ifdef _LOGGER_COUT
      (std::cout << ... << args);
      std::cout << "\n";
#endif
    }

Referenced by Decision::update().

Here is the caller graph for this function:

log_debug()

template<typename... Args>

void galaxis::utils::log_debug

(

const Args &...

args

)

Definition at line 62 of file logger.hpp.

                                        {
#ifdef _LOGGER_DEBUG
      std::cerr << "DEBUG: ";
      (std::cerr << ... << args);
      std::cerr << "\n";
#endif
    }

Referenced by Localizer::add_state(), VCINodeAsync::camera_publish_loop(), EuclideanDetector::cluster_data(), StopLineDetection::detect_startline(), StopLineDetection::detect_stopline(), Realsense::init(), ECUNode::motion_callback(), AutoPilotNode::on_parking_state_callback(), AutoPilotNode::on_qr_code_callback(), AutoPilotNode::on_stop_line_callback(), AutoPilotNode::on_vci_lane_detection_callback(), read_file_to_string(), and Decision::update().

Here is the caller graph for this function:

log_error()

template<typename... Args>

void galaxis::utils::log_error

(

const Args &...

args

)

Definition at line 50 of file logger.hpp.

                                        {
#ifdef _LOGGER_ERROR
      std::cerr << "ERROR: ";
      (std::cerr << ... << args);
      std::cerr << "\n";
#ifdef _LOGGER_ERROR_THROW_EXCEPTION
      throw std::runtime_error("Look at the ERROR above. (thrown from utilities/.../logger.hpp)");
#endif
#endif
    }

Referenced by GalaxisNode::enable_parameters(), galaxis::control::TwoAndAHalfCarrots::get_pose_series(), AutoPilotNode::on_vci_lane_detection_callback(), read_from_file_to_string(), RealSenseNode::RealSenseNode(), AutoPilotNode::send_controller_references(), galaxis::control::TwoAndAHalfCarrots::set_horizon(), QRCodeDetectionNode::test_detection_loop(), and write_to_file().

Here is the caller graph for this function:

log_nonewline()

template<typename... Args>

void galaxis::utils::log_nonewline

(

const Args &...

args

)

Definition at line 18 of file logger.hpp.

                                            {
#ifdef _LOGGER_INFO
      (std::clog << ... << args);
#endif
    }

Referenced by RemoteControl::loop().

Here is the caller graph for this function:

log_warning()

template<typename... Args>

void galaxis::utils::log_warning

(

const Args &...

args

)

Definition at line 41 of file logger.hpp.

                                          {
#ifdef _LOGGER_WARNING
      std::clog << "WARNING: ";
      (std::clog << ... << args);
      std::clog << "\n";
#endif
    }

Referenced by Localizer::add_state(), AlviumAsyncCamera::AlviumAsyncCamera(), AutoPilotNode::AutoPilotNode(), galaxis::control::TwoAndAHalfCarrots::calculate_best_yaw_rate(), VCINodeAsync::camera_publish_loop(), GalaxisNode::create_parameters(), CameraFrameObserver::FrameReceived(), GalaxisNode::GalaxisNode(), AutoPilot::get_drive_information(), PositionTimepointBuffer::get_last_by_time(), galaxis::control::TwoAndAHalfCarrots::get_last_first_poseseries(), galaxis::control::TwoAndAHalfCarrots::get_last_second_poseseries(), MapDataLayer::getLineObject(), LaneDetectionGT::groundtruthCallback(), Realsense::init(), AutoPilotNode::lane_detection_shared_memory_loop(), ControllerNode::loop(), LaneDetectionGT::loop(), StopLineDetection::new_parameters_callback(), RealSenseNode::new_parameters_callback(), ControllerNode::new_parameters_callback(), AutoPilotNode::new_parameters_callback(), RealSenseNode::object_detection_loop(), ControllerNode::on_measurement_callback(), AutoPilotNode::on_obstacle_callback(), AutoPilotNode::on_vci_lane_detection_callback(), AlviumAsyncCamera::pop_image(), ECUNode::publishMeasurements(), read_configuration(), AutoPilotNode::send_full_map_debug_image(), AutoPilotNode::send_map_debug_image(), galaxis::control::TwoAndAHalfCarrots::set_horizon(), Decision::set_parking_state(), AlviumAsyncCamera::start_grabbing_detached(), MapDataLayer::updateCell(), and VCINodeAsync::VCINodeAsync().

Here is the caller graph for this function:

read_configuration()

shared_ptr< std::map< std::string, std::string > > galaxis::utils::read_configuration

(

string const &

p_filepath

)

Definition at line 41 of file file.cpp.

                                                                                                  {
  shared_ptr<std::map<std::string, std::string>> contentSPtr = std::make_shared<std::map<std::string, std::string>>();
 
  ifstream file;
  file.open(p_filepath);
 
  if(file.is_open()) {
 
    string line;
    while(getline(file, line)) {
      if(!line.empty()) {
        
        size_t lineDelimiterPosition = line.find("=");
        if (lineDelimiterPosition == std::string::npos || lineDelimiterPosition == 0) continue; // continue if line not in format: key=value
 
        std::string key = line.substr(0, lineDelimiterPosition); 
        std::string value = line.substr(lineDelimiterPosition + 1, line.size());
 
        if (contentSPtr->find(key) == contentSPtr->end())
          contentSPtr->insert(std::pair<std::string, std::string>(key, value));
        else
          log_warning("[File] Duplicated Keys while loading config file: ", p_filepath);
 
      }
    }
 
  } else {
    log_warning("Failed to open file.");
    throw std::runtime_error("Failed to open file.");
  }
 
  file.close();
 
  return contentSPtr;
}

References log_warning().

Referenced by LaneDetectionGT::LaneDetectionGT().

Here is the call graph for this function:

Here is the caller graph for this function:

read_file_to_string()

shared_ptr< std::string > galaxis::utils::read_file_to_string

(

std::string

path

)

Definition at line 24 of file settings.cpp.

                                                                      {
  // open file and append to string 
  std::ifstream file;
  file.open(path);
  std::shared_ptr<std::string> fileContent = std::make_shared<std::string>();
 
  if(file.is_open()) {
    std::string line;
 
    while(getline(file, line)) {
      (*fileContent) += line;
    }
 
    file.close();
  }
  else {
    log_debug("Failed to load file: ", path);
    throw std::runtime_error("Failed to load file.");
  }
 
  return fileContent;
}

References log_debug().

Referenced by read_json_file(), and read_system_settings().

Here is the call graph for this function:

Here is the caller graph for this function:

read_from_file_to_float()

shared_ptr< vector< float > > galaxis::utils::read_from_file_to_float

(

string const &

p_filepath

)

Definition at line 29 of file file.cpp.

                                                                                        {
  auto contentStringSPtr = galaxis::utils::read_from_file_to_string(p_filepath);
 
  shared_ptr<vector<float>> contentUPtr = std::make_shared<vector<float>>();
 
  for(auto& c: *contentStringSPtr) {
    contentUPtr->push_back(stof(c));
  }
 
  return move(contentUPtr);
}

References read_from_file_to_string().

Here is the call graph for this function:

read_from_file_to_string()

shared_ptr< vector< string > > galaxis::utils::read_from_file_to_string

(

string const &

p_filepath

)

Definition at line 5 of file file.cpp.

                                                                                          {
  shared_ptr<vector<string>> contentUPtr = std::make_shared<vector<string>>();
 
  ifstream file;
  file.open(p_filepath);
 
  if(file.is_open()) {
 
    string line;
    while(getline(file, line)) {
      if(!line.empty()) {
        contentUPtr->push_back(line);
      }
    }
 
  } else {
    log_error("Failed to open file.");
  }
 
  file.close();
 
  return move(contentUPtr);
}

References log_error().

Referenced by read_from_file_to_float().

Here is the call graph for this function:

Here is the caller graph for this function:

read_json_file()

shared_ptr< json > galaxis::utils::read_json_file

(

std::string

path

)

Definition at line 5 of file settings.cpp.

                                                            {
  auto fileContentRawSPtr = read_file_to_string(path);
  
  auto filecontentSPtr = std::make_shared<json>();
  *filecontentSPtr = nlohmann::json::parse(*fileContentRawSPtr);
 
  return filecontentSPtr;
}

References read_file_to_string().

Referenced by AutoPilotNode::AutoPilotNode(), ControllerNode::ControllerNode(), GalaxisNode::GalaxisNode(), RealSenseNode::RealSenseNode(), and StopLineDetection::StopLineDetection().

Here is the call graph for this function:

Here is the caller graph for this function:

read_system_settings()

shared_ptr< json > galaxis::utils::read_system_settings

(

std::string

key

)

Definition at line 14 of file settings.cpp.

                                                                 {
  std::string systemSettingsPath = "settings/runtime_system.json";
 
  auto fileContentRawSPtr = read_file_to_string(systemSettingsPath);
 
  auto filecontentSPtr = std::make_shared<json>();
  *filecontentSPtr = nlohmann::json::parse(*fileContentRawSPtr)[key];
  return filecontentSPtr;
}

References read_file_to_string().

Referenced by GalaxisNode::enable_parameters().

Here is the call graph for this function:

Here is the caller graph for this function:

write_to_file()

template<typename T >

void galaxis::utils::write_to_file	(	string const &	p_filepath,
		std::vector< std::vector< T >> const &	p_data,
		string const &	p_delimiter = ","
	)

Definition at line 21 of file file.hpp.

                                                                                                                         {
      ofstream file;
      file.open(p_filepath);
 
      if(file.is_open()) {
 
        for(auto const& r: p_data) {
          for(auto const& c: r) {
            file << c << p_delimiter;
          }
          file << "\n";
        }
 
      } else {
        log_error("Failed to write file.");
      }
 
      file.close();
    }

References log_error().

Here is the call graph for this function: