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