diff --git a/src/arussim/include/arussim/supervisor.hpp b/src/arussim/include/arussim/supervisor.hpp index 7f6f806..7f13cc8 100644 --- a/src/arussim/include/arussim/supervisor.hpp +++ b/src/arussim/include/arussim/supervisor.hpp @@ -73,6 +73,15 @@ class Supervisor : public rclcpp::Node */ std::string get_config_params(); + /** + * @brief Find the laptimes folder of the ARUSSim sources. + * + * Walks up from the executable until a folder contains src/ARUSSim/src/arussim/laptimes. + * + * @return std::filesystem::path Path to the laptimes folder, or empty if it is not found. + */ + std::filesystem::path find_laptimes_dir(); + //Publishers and subscribers diff --git a/src/arussim/src/supervisor.cpp b/src/arussim/src/supervisor.cpp index a748622..2ba6421 100644 --- a/src/arussim/src/supervisor.cpp +++ b/src/arussim/src/supervisor.cpp @@ -85,19 +85,26 @@ void Supervisor::tpl_signal_callback([[maybe_unused]] const std_msgs::msg::Bool: if (!started_){ RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "%sLap started%s", green.c_str(), reset.c_str()); prev_time_ = this->get_clock()->now().seconds(); - auto ws_path = std::filesystem::canonical("/proc/self/exe"); - for (int i = 0; i < 3; ++i) ws_path = ws_path.parent_path(); - file_path_ = ws_path / "src/ARUSSim/src/arussim/laptimes" / (circuit_ + ".csv"); + std::filesystem::path laptimes_dir = find_laptimes_dir(); + file_path_.clear(); + if (laptimes_dir.empty()) { + RCLCPP_WARN(this->get_logger(), + "Laptimes folder not found: the best lap time will not be saved"); + } else { + file_path_ = laptimes_dir / (circuit_ + ".csv"); + } current_best_time_ = 0.0; // Reads the file if it exists and overwrites it if the time done in simulation is better than the stored one. - if (std::filesystem::exists(file_path_) && std::filesystem::is_regular_file(file_path_)) - { - std::ifstream file(file_path_); - + std::ifstream file; + if (!file_path_.empty() && std::filesystem::is_regular_file(file_path_)) { + file.open(file_path_); if (!file.is_open()) { - throw std::runtime_error("Could not open CSV file"); + RCLCPP_WARN(this->get_logger(), "Could not open %s", file_path_.c_str()); } + } + if (file.is_open()) + { std::string line; std::string last_line; @@ -119,7 +126,12 @@ void Supervisor::tpl_signal_callback([[maybe_unused]] const std_msgs::msg::Bool: std::getline(ss, cones, ','); std::getline(ss, time, ','); - current_best_time_ = std::stod(time); + try { + current_best_time_ = std::stod(time); + } catch (const std::exception&) { + RCLCPP_WARN(this->get_logger(), "Could not read the best lap time from %s", + file_path_.c_str()); + } } } if(circuit_ == "skidpad") time_cone_penalty_ = 0.2; @@ -165,7 +177,7 @@ void Supervisor::tpl_signal_callback([[maybe_unused]] const std_msgs::msg::Bool: row_values.push_back(std::to_string(n_total_cones_hit_)); csv_generator_->write_row("lap_time,total_hit_cones", row_values); } - if (std::filesystem::exists(file_path_) && std::filesystem::is_regular_file(file_path_)) + if (!file_path_.empty() && std::filesystem::is_regular_file(file_path_)) { if(current_best_time_ <= 0.0 || current_best_time_ > best_time_){ std::ofstream file(file_path_); @@ -178,18 +190,20 @@ void Supervisor::tpl_signal_callback([[maybe_unused]] const std_msgs::msg::Bool: current_best_time_ = best_time_; } } - else // Creates a .csv file and adds the time done in simulation. + else if (!file_path_.empty()) // Creates a .csv file and adds the time done in simulation. { std::ofstream file(file_path_, std::ios::out); if (!file.is_open()) { - throw std::runtime_error("Could not create CSV file"); + RCLCPP_WARN(this->get_logger(), "Could not create %s: best lap time not saved", + file_path_.c_str()); + } else { + file << "lap,cones,best_time\n"; + file << lap_time_ << "," << cones_hitted_ << "," << best_time_ << "\n"; + file << "---PARAM_DUMP_BEGIN---\n"; + file << parameters_dump_ << "\n"; + file << "---PARAM_DUMP_END---\n"; + current_best_time_ = best_time_; } - file << "lap,cones,best_time\n"; - file << lap_time_ << "," << cones_hitted_ << "," << best_time_ << "\n"; - file << "---PARAM_DUMP_BEGIN---\n"; - file << parameters_dump_ << "\n"; - file << "---PARAM_DUMP_END---\n"; - current_best_time_ = best_time_; } } speed_multiplier_list_.clear(); @@ -277,6 +291,29 @@ std::string Supervisor::get_config_params() return result; } +/** + * @brief Find the laptimes folder of the ARUSSim sources. + * + * Walks up from the executable until a folder contains src/ARUSSim/src/arussim/laptimes. With + * --symlink-install the executable resolves to build/arussim and without it to + * install/arussim/lib/arussim, so a fixed number of levels only works in one of the two cases. + * + * @return std::filesystem::path Path to the laptimes folder, or empty if it is not found. + */ +std::filesystem::path Supervisor::find_laptimes_dir() +{ + const std::filesystem::path laptimes = "src/ARUSSim/src/arussim/laptimes"; + std::error_code error; + std::filesystem::path dir = std::filesystem::canonical("/proc/self/exe", error); + if (error) return {}; + while (dir.has_parent_path() && dir != dir.parent_path()) { + dir = dir.parent_path(); + if (std::filesystem::is_directory(dir / laptimes, error)) return dir / laptimes; + } + return {}; +} + + /** * @brief Main function for the supervisor node *