add config parsers and configs in YAML

rbs_mill_assist/include/rbs_mill_assist/places_config.hpp

@@ -0,0 +1,118 @@
+#include "geometry_msgs/msg/pose.hpp"
+#include <filesystem>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/node.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <yaml-cpp/node/node.h>
+#include <yaml-cpp/node/parse.h>
+#include <yaml-cpp/yaml.h>
+
+class PlacesConfig {
+public:
+  PlacesConfig(const std::string &places_filepath) {
+    loadConfig(places_filepath);
+  }
+
+  inline std::vector<std::string> getMeshFilenames() {
+    return _model_mesh_filenames;
+  }
+  inline std::vector<geometry_msgs::msg::Pose> getMeshPoses(){
+    return _model_poses;
+  }
+  inline std::vector<std::string> getMeshFilepaths() {
+    return _model_mesh_filepaths;
+  }
+  inline std::vector<std::string> getModelNames() {
+    return _model_names;
+  }
+
+private:
+  std::vector<std::string> _model_names;
+  std::vector<std::string> _model_mesh_filenames;
+  std::vector<geometry_msgs::msg::Pose> _model_poses;
+  std::vector<std::string> _model_mesh_filepaths;
+
+  bool isQuat(const YAML::Node &node) {
+    if (node["x"] && node["y"] && node["z"] && node["w"]) {
+      return true;
+    } else if (node["r"] && node["p"] && node["y"]) {
+      return false;
+    }
+    throw std::invalid_argument(
+        "Provided YAML Node is not quat or euler angles");
+  }
+
+  geometry_msgs::msg::Quaternion toQuat(const YAML::Node &node) {
+    geometry_msgs::msg::Quaternion quat;
+    if (isQuat(node)) {
+      quat.x = node["x"].as<double>();
+      quat.y = node["y"].as<double>();
+      quat.z = node["z"].as<double>();
+      quat.w = node["w"].as<double>();
+    } else {
+      tf2::Quaternion tf2_quat;
+      tf2_quat.setRPY(node["r"].as<double>(), node["p"].as<double>(),
+                      node["y"].as<double>());
+      quat.x = tf2_quat.x();
+      quat.y = tf2_quat.y();
+      quat.z = tf2_quat.z();
+      quat.w = tf2_quat.w();
+    }
+    return quat;
+  }
+  void loadConfig(const std::string &places_config_filepath) {
+    YAML::Node places_config_node = YAML::LoadFile(places_config_filepath);
+
+    std::string model_dir;
+    if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
+      model_dir = node.as<std::string>();
+    }
+
+    if (!places_config_node["models"] ||
+        !places_config_node["models"].IsSequence()) {
+      throw std::runtime_error(
+          "No valid 'models' section found in places.yaml");
+    }
+
+    for (const auto &model : places_config_node["models"]) {
+      if (!model["name"] || !model["mesh_filename"] || !model["pose"]) {
+        throw std::runtime_error(
+            "Missing required model parameters in places.yaml");
+      }
+
+      std::string model_name = model["name"].as<std::string>();
+      std::string model_mesh_filename =
+          model["mesh_filename"].as<std::string>();
+
+      _model_names.push_back(std::move(model_name));
+      _model_mesh_filenames.push_back(std::move(model_mesh_filename));
+
+      if (!model_dir.empty()) {
+        std::filesystem::path mesh_path = std::filesystem::path(model_dir) /
+                                          _model_names.back() / "meshes" /
+                                          _model_mesh_filenames.back();
+        _model_mesh_filepaths.push_back(mesh_path.string());
+      }
+
+      geometry_msgs::msg::Pose model_pose;
+
+      auto position = model["pose"]["position"];
+      if (!position) {
+        throw std::runtime_error("Define the model position in places.yaml");
+      }
+
+      model_pose.position.x = position["x"].as<double>();
+      model_pose.position.y = position["y"].as<double>();
+      model_pose.position.z = position["z"].as<double>();
+
+      if (auto orientation = model["pose"]["orientation"]; orientation) {
+        model_pose.orientation = toQuat(orientation);
+      } else {
+        RCLCPP_WARN(rclcpp::get_logger("planning_scene_loader"),
+                    "Model orientation is not set. Using default values.");
+      }
+
+      _model_poses.push_back(std::move(model_pose));
+    }
+  }
+};

rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp

@@ -0,0 +1,79 @@
+#include "geometry_msgs/msg/pose.hpp"
+#include <filesystem>
+#include <rclcpp/logger.hpp>
+#include <rclcpp/node.hpp>
+#include <stdexcept>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <yaml-cpp/yaml.h>
+
+class RefFramesConfig {
+public:
+  RefFramesConfig(const std::string &ref_frames_filepath) {
+    loadConfig(ref_frames_filepath);
+  }
+  inline std::vector<std::string> getRefFrameNames() {
+    return _ref_frame_names;
+  }
+  inline std::vector<std::string> getRefFrameParents() {
+    return _ref_frame_parents;
+  }
+  inline std::vector<geometry_msgs::msg::Pose> getRefFramePoses() {
+    return _ref_frame_poses;
+  }
+
+private:
+  std::vector<std::string> _ref_frame_names;
+  std::vector<std::string> _ref_frame_parents;
+  std::vector<geometry_msgs::msg::Pose> _ref_frame_poses;
+
+  geometry_msgs::msg::Quaternion toQuat(const YAML::Node &node) {
+    geometry_msgs::msg::Quaternion quat;
+    if (node["x"] && node["y"] && node["z"] && node["w"]) {
+      quat.x = node["x"].as<double>();
+      quat.y = node["y"].as<double>();
+      quat.z = node["z"].as<double>();
+      quat.w = node["w"].as<double>();
+    } else if (node["r"] && node["p"] && node["y"]) {
+      tf2::Quaternion tf2_quat;
+      tf2_quat.setRPY(node["r"].as<double>(), node["p"].as<double>(),
+                      node["y"].as<double>());
+      quat.x = tf2_quat.x();
+      quat.y = tf2_quat.y();
+      quat.z = tf2_quat.z();
+      quat.w = tf2_quat.w();
+    }
+    return quat;
+  }
+
+  void parseRefFrames(const YAML::Node &node) {
+    for (const auto &frame : node) {
+      if (!frame["name"] || !frame["relative_to"] || !frame["pose"]) {
+        throw std::runtime_error(
+            "Missing required ref_frame parameters in YAML");
+      }
+      _ref_frame_names.push_back(frame["name"].as<std::string>());
+      _ref_frame_parents.push_back(frame["relative_to"].as<std::string>());
+
+      geometry_msgs::msg::Pose frame_pose;
+      frame_pose.position.x = frame["pose"]["position"]["x"].as<double>();
+      frame_pose.position.y = frame["pose"]["position"]["y"].as<double>();
+      frame_pose.position.z = frame["pose"]["position"]["z"].as<double>();
+
+      if (auto orientation = frame["pose"]["orientation"]; orientation) {
+        frame_pose.orientation = toQuat(orientation);
+      }
+      _ref_frame_poses.push_back(frame_pose);
+    }
+  }
+
+  void loadConfig(const std::string &ref_frames_filepath) {
+    YAML::Node config = YAML::LoadFile(ref_frames_filepath);
+
+    if (config["ref_frames"] && config["ref_frames"].IsSequence()) {
+      parseRefFrames(config["ref_frames"]);
+    } else {
+      throw std::runtime_error("No ref_frames in config");
+    }
+
+  }
+};

rbs_mill_assist/world/config/places.yaml

@@ -0,0 +1,50 @@
+model_dir: package://rbs_mill_assist/assets/
+models:
+  - name: bunker
+    mesh_filename: Bunker_50mm_height_for_labels_60х40.STL
+    #relative_to: world
+    pose:
+      position:
+        x: -0.310
+        y: 0.250
+        z: 0.0
+      orientation:
+        r: 0.0
+        p: 0.0
+        y: 1.57
+
+  - name: bunker_4_slots
+    mesh_filename: Bunker_50mm_height_for_labels_60х40_4.STL
+    pose:
+      position:
+        x: -0.150
+        y: 0.250
+        z: 0.0
+      orientation:
+        r: 0.0
+        p: 0.0
+        y: 1.57
+
+  - name: conductor
+    mesh_filename: conductor.stl
+    pose:
+      position:
+        x: 0.350
+        y: -0.170
+        z: 0.01
+      # orientation:
+      #   r: 0.0
+      #   p: 0.0
+      #   y: 0.0
+
+  - name: laser
+    mesh_filename: laser.dae
+    pose:
+      position:
+        x: 0.350
+        y: -0.170
+        z: 0.0
+      orientation:
+        r: 0.0
+        p: 0.0
+        y: 3.14159

rbs_mill_assist/world/config/slots.yaml

@@ -0,0 +1,29 @@
+ref_frames:
+  - name: "slot_1"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: 0.066
+        z: 0.0
+  - name: "slot_2"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: 0.022
+        z: 0.0
+  - name: "slot_3"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: -0.022
+        z: 0.0
+  - name: "slot_4"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: -0.066
+        z: 0.0