add config parsers and configs in YAML
This commit is contained in:
parent
4b457c2e2e
commit
cfbf4b0755
4 changed files with 276 additions and 0 deletions
118
rbs_mill_assist/include/rbs_mill_assist/places_config.hpp
Normal file
118
rbs_mill_assist/include/rbs_mill_assist/places_config.hpp
Normal file
|
@ -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));
|
||||
}
|
||||
}
|
||||
};
|
|
@ -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");
|
||||
}
|
||||
|
||||
}
|
||||
};
|
50
rbs_mill_assist/world/config/places.yaml
Normal file
50
rbs_mill_assist/world/config/places.yaml
Normal file
|
@ -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
|
29
rbs_mill_assist/world/config/slots.yaml
Normal file
29
rbs_mill_assist/world/config/slots.yaml
Normal file
|
@ -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
|
Loading…
Add table
Add a link
Reference in a new issue