utils.hpp

Go to the documentation of this file.

#pragma once
 
#include <ros/ros.h>
 
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <chrono>
#include <filesystem>
#include <fstream>
#include <regex>
#include <set>
#include <string>
#include <vector>
 
#include "common_lib/structures/cone.hpp"
#include "common_lib/structures/path_point.hpp"
#include "gtest/gtest.h"
#include "planning/path_calculation.hpp"
#include "planning/planning.hpp"
#include "planning/smoothing.hpp"
#include "planning/velocity_planning.hpp"
#include "rclcpp/rclcpp.hpp"
#include "utils/files.hpp"
#include "utils/splines.hpp"
 
using testing::Eq;
namespace fs = std::filesystem;
 
std::ifstream open_read_file(const std::string &filename);
 
std::vector<common_lib::structures::Cone> cone_vector_from_file(const std::string &path);
 
std::pair<std::vector<common_lib::structures::Cone>, std::vector<common_lib::structures::Cone>>
track_from_file(const std::string &path);
 
std::vector<common_lib::structures::PathPoint> path_from_file(const std::string &path);
 
void extract_info(const std::string_view &filename_view, int &size, int &n_outliers);
 
float consecutive_max_distance(const std::vector<common_lib::structures::Cone> &cones);
 
std::vector<std::pair<double, double>> order_vector_of_pairs(
    const std::vector<std::pair<double, double>> &vec);
 
std::string get_current_date_time_as_string();
 
float round_n(float num, int decimal_places);
 
struct PathPointHash {
  std::size_t operator()(const common_lib::structures::PathPoint &p) const {
    // Adjust the hash function for a bigger tolerance
    std::size_t h1 = std::hash<long>()(static_cast<long>(std::round(p.position.x / 0.5)));
    std::size_t h2 = std::hash<long>()(static_cast<long>(std::round(p.position.y / 0.5)));
    return h1 ^ (h2 << 1);  // Combine the hashes using bitwise shift to reduce collisions
  }
};
 
struct PathPointEqual {
  bool operator()(const common_lib::structures::PathPoint &p1,
                  const common_lib::structures::PathPoint &p2) const {
    // Use a bigger tolerance for equality comparison
    bool result = std::abs(p1.position.x - p2.position.x) < 0.5 &&
                  std::abs(p1.position.y - p2.position.y) < 0.5;
    return result;
  }
};