diff --git a/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp b/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp new file mode 100644 index 0000000..ce0de96 --- /dev/null +++ b/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp @@ -0,0 +1,118 @@ +#include "geometry_msgs/msg/pose.hpp" +#include +#include +#include +#include +#include +#include +#include + +class PlacesConfig { +public: + PlacesConfig(const std::string &places_filepath) { + loadConfig(places_filepath); + } + + inline std::vector getMeshFilenames() { + return _model_mesh_filenames; + } + inline std::vector getMeshPoses(){ + return _model_poses; + } + inline std::vector getMeshFilepaths() { + return _model_mesh_filepaths; + } + inline std::vector getModelNames() { + return _model_names; + } + +private: + std::vector _model_names; + std::vector _model_mesh_filenames; + std::vector _model_poses; + std::vector _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(); + quat.y = node["y"].as(); + quat.z = node["z"].as(); + quat.w = node["w"].as(); + } else { + tf2::Quaternion tf2_quat; + tf2_quat.setRPY(node["r"].as(), node["p"].as(), + node["y"].as()); + 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(); + } + + 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 model_mesh_filename = + model["mesh_filename"].as(); + + _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(); + model_pose.position.y = position["y"].as(); + model_pose.position.z = position["z"].as(); + + 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)); + } + } +}; diff --git a/rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp b/rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp new file mode 100644 index 0000000..b3f233a --- /dev/null +++ b/rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp @@ -0,0 +1,79 @@ +#include "geometry_msgs/msg/pose.hpp" +#include +#include +#include +#include +#include +#include + +class RefFramesConfig { +public: + RefFramesConfig(const std::string &ref_frames_filepath) { + loadConfig(ref_frames_filepath); + } + inline std::vector getRefFrameNames() { + return _ref_frame_names; + } + inline std::vector getRefFrameParents() { + return _ref_frame_parents; + } + inline std::vector getRefFramePoses() { + return _ref_frame_poses; + } + +private: + std::vector _ref_frame_names; + std::vector _ref_frame_parents; + std::vector _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(); + quat.y = node["y"].as(); + quat.z = node["z"].as(); + quat.w = node["w"].as(); + } else if (node["r"] && node["p"] && node["y"]) { + tf2::Quaternion tf2_quat; + tf2_quat.setRPY(node["r"].as(), node["p"].as(), + node["y"].as()); + 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()); + _ref_frame_parents.push_back(frame["relative_to"].as()); + + geometry_msgs::msg::Pose frame_pose; + frame_pose.position.x = frame["pose"]["position"]["x"].as(); + frame_pose.position.y = frame["pose"]["position"]["y"].as(); + frame_pose.position.z = frame["pose"]["position"]["z"].as(); + + 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"); + } + + } +}; diff --git a/rbs_mill_assist/world/config/places.yaml b/rbs_mill_assist/world/config/places.yaml new file mode 100644 index 0000000..1330132 --- /dev/null +++ b/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 diff --git a/rbs_mill_assist/world/config/slots.yaml b/rbs_mill_assist/world/config/slots.yaml new file mode 100644 index 0000000..64c657c --- /dev/null +++ b/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