robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

rbs_utils in env_manager and clear code

Browse source
This commit is contained in:
Ilya Uraev 2023-12-14 12:00:35 +03:00
parent 0588a5f6c6
commit 34c8961723
23 changed files with 536 additions and 237 deletions
Download patch file Download diff file Expand all files Collapse all files

1
rbs_utils/CMakeLists.txt
View file

@ -56,6 +56,5 @@ if(BUILD_TESTING)
ament_lint_auto_find_test_dependencies()
endif()
# ament_export_dependencies()
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_package()

28
rbs_utils/include/rbs_utils/rbs_utils.hpp
View file

@ -8,6 +8,7 @@
#include <nlohmann/json.hpp>
#include <rclcpp/node.hpp>
#include <string>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <unordered_map>
@ -17,29 +18,50 @@ const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR");
namespace rbs_utils {
struct EnvModels {
std::string model_name;
std::string mesh_path;
std::vector<double> model_inertia;
std::vector<double> model_pose;
double mass;
};
class AssemblyConfigLoader {
public:
AssemblyConfigLoader(const std::string &t_assembly_dir);
inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>>
getAssemblyFileData();
getAssemblyFileData() {
return m_env_vars;
}
void setTfData();
tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
inline std::shared_ptr<std::vector<EnvModels>> getEnvModels() {
return m_env_models;
}
std::vector<std::string> getSceneModelNames();
private:
std::shared_ptr<std::unordered_map<std::string, nlohmann::json>> m_env_vars;
std::vector<std::string> m_env_files;
std::vector<std::ifstream> m_env_paths;
std::shared_ptr<std::vector<EnvModels>> m_env_models{};
std::string m_assembly_dir;
std::unique_ptr<tf2_ros::Buffer> m_tf_buffer;
void readAssemblyFileData(const std::string &filename,
const std::string &filepath);
tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json);
tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json,
const std::string &model_name);
void setTfFromDb(const std::string &filename);
double convertToDouble(const nlohmann::json &value);
void readModelConfigs();
EnvModels parseModelData(const std::string &jsonFilePath);
};
class StaticFramePublisher : public rclcpp::Node {

232
rbs_utils/src/rbs_utils.cpp
View file

@ -1,8 +1,10 @@
#include "rbs_utils/rbs_utils.hpp"
#include <fstream>
#include <iostream>
#include <iterator>
#include <memory>
#include <nlohmann/json_fwd.hpp>
#include <ostream>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
@ -10,21 +12,28 @@
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <unordered_map>
#include <vector>
namespace rbs_utils {
AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
: m_env_vars(
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()) {
if (!t_assembly_dir.empty()) {
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()),
m_env_models(std::make_shared<std::vector<EnvModels>>()),
m_assembly_dir(t_assembly_dir) {
if (!m_assembly_dir.empty()) {
std::vector<std::string> filenames = {"robossembler_db", "sequences"};
for (auto &filename : filenames) {
std::string filepath =
env_dir + "/" + t_assembly_dir + "/" + filename + ".json";
env_dir + "/" + m_assembly_dir + "/" + filename + ".json";
m_env_files.push_back(filepath);
readAssemblyFileData(filename, filepath);
}
readModelConfigs();
} else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Assembly dir is not set");
}
}
@ -50,19 +59,112 @@ void AssemblyConfigLoader::readAssemblyFileData(const std::string &filename,
}
}
inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>>
AssemblyConfigLoader::getAssemblyFileData() {
return m_env_vars;
EnvModels
AssemblyConfigLoader::parseModelData(const std::string &json_file_path) {
nlohmann::json json_content{};
try {
std::ifstream file(json_file_path);
if (!file.is_open()) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s",
json_file_path.c_str());
return EnvModels{};
}
json_content = nlohmann::json::parse(file);
} catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Error parsing JSON in file %s: %s", json_file_path.c_str(),
e.what());
} catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Exception reading file %s: %s", json_file_path.c_str(),
e.what());
}
EnvModels envModel;
if (!json_content.empty()) {
envModel.model_name = json_content.at("name").get<std::string>();
envModel.mesh_path = env_dir + "/" + m_assembly_dir + "/" +
json_content.at("stl").get<std::string>();
envModel.model_inertia = {convertToDouble(json_content.at("ixx")),
convertToDouble(json_content.at("ixy")),
convertToDouble(json_content.at("iyy")),
convertToDouble(json_content.at("ixz")),
convertToDouble(json_content.at("izz")),
convertToDouble(json_content.at("iyz"))};
envModel.model_pose = {convertToDouble(json_content.at("posX")),
convertToDouble(json_content.at("posY")),
convertToDouble(json_content.at("posZ")),
convertToDouble(json_content.at("eulerX")),
convertToDouble(json_content.at("eulerY")),
convertToDouble(json_content.at("eulerZ"))};
envModel.mass = convertToDouble(json_content.at("massSDF"));
}
return envModel;
}
void AssemblyConfigLoader::setTfData() {
void AssemblyConfigLoader::readModelConfigs() {
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json &db_json = (*m_env_vars)["robossembler_db"];
const std::string assets_folder =
env_dir + "/" + m_assembly_dir + "/" +
db_json.at("assets_db").get<std::string>();
for (const auto &entry :
std::filesystem::directory_iterator(assets_folder)) {
if (entry.is_regular_file() && entry.path().extension() == ".json") {
EnvModels model = parseModelData(entry.path().string());
m_env_models->push_back(model);
}
}
}
}
std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
std::vector<std::string> model_names;
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json &json = (*m_env_vars)["robossembler_db"];
tf2_msgs::msg::TFMessage tf_msg = parseJsonToTFMessage(json);
// Extract names from "relativeParts"
if (json.find("relativeParts") != json.end()) {
const auto &relative_parts = json["relativeParts"];
for (const auto &part : relative_parts) {
model_names.push_back(part["name"]);
}
}
// Extract names from "graspPoseModels"
if (json.find("graspPoseModels") != json.end()) {
const auto &grasp_pose_models = json["graspPoseModels"];
for (const auto &pose_model : grasp_pose_models) {
model_names.push_back(pose_model["name"]);
}
}
} else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Key 'robossembler_db' not found in m_env_vars.");
return model_names;
}
// Sort and remove duplicates
std::sort(model_names.begin(), model_names.end());
model_names.erase(std::unique(model_names.begin(), model_names.end()),
model_names.end());
return model_names;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::getTfData(const std::string &model_name) {
tf2_msgs::msg::TFMessage tf_msg{};
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json &json = (*m_env_vars)["robossembler_db"];
tf_msg = parseJsonToTFMessage(json, model_name);
// Output position information to console
for (const auto &transform : tf_msg.transforms) {
RCLCPP_DEBUG_STREAM(
RCLCPP_INFO_STREAM(
rclcpp::get_logger("rbs_utils"),
"Frame ID: " << transform.header.frame_id << ", Child Frame ID: "
<< transform.child_frame_id << ", Translation: ["
@ -70,77 +172,91 @@ void AssemblyConfigLoader::setTfData() {
<< transform.transform.translation.y << ", "
<< transform.transform.translation.z << "]");
}
auto r = std::make_shared<StaticFramePublisher>(tf_msg);
// auto r = std::make_shared<StaticFramePublisher>(tf_msg);
} else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Key 'robossembler_db' not found in m_env_vars.");
}
return tf_msg;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json) {
AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
const std::string &model_name) {
tf2_msgs::msg::TFMessage tf_msg;
// Add absolute part to TFMessage
geometry_msgs::msg::TransformStamped absolute_transform;
absolute_transform.header.frame_id = "world";
absolute_transform.child_frame_id =
json.at("absolutePart").get<std::string>();
absolute_transform.transform.translation.x = -1.0;
absolute_transform.transform.translation.y = 0.0;
absolute_transform.transform.translation.z = 0.0;
absolute_transform.transform.rotation.w = 1.0;
absolute_transform.transform.rotation.x = 0.0;
absolute_transform.transform.rotation.y = 0.0;
absolute_transform.transform.rotation.z = 0.0;
json.at("absolutePart").at("name").get<std::string>();
absolute_transform.transform.translation.x =
convertToDouble(json.at("absolutePart").at("position").at("x"));
absolute_transform.transform.translation.y =
convertToDouble(json.at("absolutePart").at("position").at("y"));
absolute_transform.transform.translation.z =
convertToDouble(json.at("absolutePart").at("position").at("z"));
absolute_transform.transform.rotation.w =
convertToDouble(json.at("absolutePart").at("quaternion").at("qw"));
absolute_transform.transform.rotation.x =
convertToDouble(json.at("absolutePart").at("quaternion").at("qx"));
absolute_transform.transform.rotation.y =
convertToDouble(json.at("absolutePart").at("quaternion").at("qy"));
absolute_transform.transform.rotation.z =
convertToDouble(json.at("absolutePart").at("quaternion").at("qz"));
tf_msg.transforms.push_back(absolute_transform);
// Add relative parts to TFMessage
for (const auto &relative_part : json.at("relativeParts")) {
geometry_msgs::msg::TransformStamped relative_transform;
relative_transform.header.frame_id =
json.at("absolutePart").get<std::string>();
relative_transform.child_frame_id =
relative_part.at("name").get<std::string>();
relative_transform.transform.translation.x =
convertToDouble(relative_part.at("position").at("x"));
relative_transform.transform.translation.y =
convertToDouble(relative_part.at("position").at("y"));
relative_transform.transform.translation.z =
convertToDouble(relative_part.at("position").at("z"));
relative_transform.transform.rotation.w =
convertToDouble(relative_part.at("quaternion").at("qw"));
relative_transform.transform.rotation.x =
convertToDouble(relative_part.at("quaternion").at("qx"));
relative_transform.transform.rotation.y =
convertToDouble(relative_part.at("quaternion").at("qy"));
relative_transform.transform.rotation.z =
convertToDouble(relative_part.at("quaternion").at("qz"));
tf_msg.transforms.push_back(relative_transform);
if (relative_part.at("name").get<std::string>() == model_name) {
geometry_msgs::msg::TransformStamped relative_transform;
relative_transform.header.frame_id =
relative_part.at("relativeAt").get<std::string>();
relative_transform.child_frame_id =
relative_part.at("name").get<std::string>();
relative_transform.transform.translation.x =
convertToDouble(relative_part.at("position").at("x"));
relative_transform.transform.translation.y =
convertToDouble(relative_part.at("position").at("y"));
relative_transform.transform.translation.z =
convertToDouble(relative_part.at("position").at("z"));
relative_transform.transform.rotation.w =
convertToDouble(relative_part.at("quaternion").at("qw"));
relative_transform.transform.rotation.x =
convertToDouble(relative_part.at("quaternion").at("qx"));
relative_transform.transform.rotation.y =
convertToDouble(relative_part.at("quaternion").at("qy"));
relative_transform.transform.rotation.z =
convertToDouble(relative_part.at("quaternion").at("qz"));
tf_msg.transforms.push_back(relative_transform);
}
}
// Add grasp pose models to TFMessage
for (const auto &grasp_pose : json.at("graspPoseModels")) {
geometry_msgs::msg::TransformStamped grasp_transform;
grasp_transform.header.frame_id =
json.at("absolutePart").get<std::string>();
grasp_transform.child_frame_id = grasp_pose.at("name").get<std::string>();
grasp_transform.transform.translation.x =
convertToDouble(grasp_pose.at("position").at("x"));
grasp_transform.transform.translation.y =
convertToDouble(grasp_pose.at("position").at("y"));
grasp_transform.transform.translation.z =
convertToDouble(grasp_pose.at("position").at("z"));
grasp_transform.transform.rotation.w =
convertToDouble(grasp_pose.at("quaternion").at("qw"));
grasp_transform.transform.rotation.x =
convertToDouble(grasp_pose.at("quaternion").at("qx"));
grasp_transform.transform.rotation.y =
convertToDouble(grasp_pose.at("quaternion").at("qy"));
grasp_transform.transform.rotation.z =
convertToDouble(grasp_pose.at("quaternion").at("qz"));
tf_msg.transforms.push_back(grasp_transform);
if (grasp_pose.at("name").get<std::string>() == model_name) {
geometry_msgs::msg::TransformStamped grasp_transform;
grasp_transform.header.frame_id =
grasp_pose.at("name").get<std::string>();
grasp_transform.child_frame_id =
grasp_pose.at("name").get<std::string>() + "_grasp";
grasp_transform.transform.translation.x =
convertToDouble(grasp_pose.at("position").at("x"));
grasp_transform.transform.translation.y =
convertToDouble(grasp_pose.at("position").at("y"));
grasp_transform.transform.translation.z =
convertToDouble(grasp_pose.at("position").at("z"));
grasp_transform.transform.rotation.w =
convertToDouble(grasp_pose.at("quaternion").at("qw"));
grasp_transform.transform.rotation.x =
convertToDouble(grasp_pose.at("quaternion").at("qx"));
grasp_transform.transform.rotation.y =
convertToDouble(grasp_pose.at("quaternion").at("qy"));
grasp_transform.transform.rotation.z =
convertToDouble(grasp_pose.at("quaternion").at("qz"));
tf_msg.transforms.push_back(grasp_transform);
}
}
return tf_msg;