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

update main

Browse source
This commit is contained in:
Ilya Uraev 2024-04-22 15:06:46 +03:00
parent d5e7768d90
commit e8ea09e020
20 changed files with 315 additions and 168 deletions
Download patch file Download diff file Expand all files Collapse all files

11
rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp
View file

@ -12,13 +12,17 @@
#include "tf2_ros/static_transform_broadcaster.h"
#include <Eigen/Core>
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__struct.hpp>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
#include <rclcpp/time.hpp>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <unordered_map>
const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR");
@ -50,14 +54,17 @@ public:
&t_clock_node_interface);
tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
tf2_msgs::msg::TFMessage getTfDataAllPossible();
std::vector<std::string> getSceneModelNames();
void printWorkspace();
geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory();
geometry_msgs::msg::Pose
transformTrajectory(const geometry_msgs::msg::TransformStamped &pose);
transformTrajectory(const geometry_msgs::msg::Pose &pose);
void saveRbsConfig();
tf2_msgs::msg::TFMessage getAdditionalPoses();
private:
std::vector<std::string> m_env_files;
@ -69,7 +76,7 @@ private:
rclcpp::Clock::SharedPtr m_clock;
void parseRbsDb(const std::string &filepath);
tf2_msgs::msg::TFMessage getWorkspace();
geometry_msgs::msg::PoseArray getWorkspace();
geometry_msgs::msg::Transform
createTransform(const geometry_msgs::msg::Pose &pose);
};

191
rbs_utils/rbs_utils/src/rbs_utils.cpp
View file

@ -1,12 +1,15 @@
#include "rbs_utils/rbs_utils.hpp"
#include <Eigen/src/Geometry/Transform.h>
#include <dynmsg/typesupport.hpp>
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__struct.hpp>
#include <rbs_utils_interfaces/msg/detail/assembly_config__traits.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
#include <string>
#include <strstream>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
@ -27,7 +30,7 @@ AssemblyConfigLoader::AssemblyConfigLoader(
m_logger(t_logging_interface->get_logger()),
m_clock(t_clock_interface->get_clock()) {
if (!m_assembly_dir.empty()) {
std::vector<std::string> filenames = {"robossembler_db"};
std::vector<std::string> filenames = {"rbs_db"};
for (auto &filename : filenames) {
std::string filepath =
env_dir + "/" + m_assembly_dir + "/" + filename + ".yaml";
@ -49,18 +52,22 @@ void AssemblyConfigLoader::parseRbsDb(const std::string &filepath) {
InterfaceTypeName interface{"rbs_utils_interfaces", "AssemblyConfig"};
rosmsg.type_info = dynmsg::cpp::get_type_info(interface);
void * ros_msg = reinterpret_cast<void *>(&m_assembly_config);
dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info, asm_config_string, ros_msg);
void *ros_msg = reinterpret_cast<void *>(&m_assembly_config);
dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info,
asm_config_string, ros_msg);
// auto file_cont = rbs_utils_interfaces::msg::to_yaml(m_assembly_config);
// std::ofstream file("rbs_db.yaml");
// if (file.is_open()) {
// file << file_cont;
// file.close();
// }
} catch (const std::exception &e) {
RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(),
e.what());
}
}
[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() {
}
[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() {}
std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
std::vector<std::string> model_names;
@ -79,37 +86,121 @@ std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
return model_names;
}
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAdditionalPoses() {
tf2_msgs::msg::TFMessage tp;
if (m_assembly_config.extra_poses.empty()) {
RCLCPP_ERROR(m_logger, "Extra poses is empty");
return tp;
}
for (auto &point : m_assembly_config.extra_poses) {
tp.transforms.emplace_back().child_frame_id = point.name;
tp.transforms.emplace_back().transform = createTransform(point.pose);
tp.transforms.emplace_back().header.stamp = m_clock->now();
tp.transforms.emplace_back().header.frame_id = "world";
}
return tp;
}
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getTfDataAllPossible() {
tf2_msgs::msg::TFMessage tp;
if (!m_assembly_config.absolute_part.empty()) {
for (auto &abs_part : m_assembly_config.absolute_part) {
geometry_msgs::msg::TransformStamped abs_transrorm_stamped;
abs_transrorm_stamped.transform = createTransform(abs_part.pose);
abs_transrorm_stamped.child_frame_id = abs_part.name;
abs_transrorm_stamped.header.frame_id = "world";
abs_transrorm_stamped.header.stamp = m_clock->now();
tp.transforms.push_back(abs_transrorm_stamped);
}
} else {
RCLCPP_ERROR(m_logger, "Absolute parts is empty size: %zu",
m_assembly_config.absolute_part.size());
}
if (!m_assembly_config.relative_part.empty()) {
for (const auto &relative_part : m_assembly_config.relative_part) {
geometry_msgs::msg::TransformStamped relative_transform_stamped;
relative_transform_stamped.transform =
createTransform(relative_part.pose);
relative_transform_stamped.child_frame_id = relative_part.name;
relative_transform_stamped.header.frame_id = relative_part.relative_at;
relative_transform_stamped.header.stamp = m_clock->now();
tp.transforms.push_back(relative_transform_stamped);
RCLCPP_INFO(m_logger, "Model name [%s]",
relative_part.relative_at.c_str());
}
}
if (!m_assembly_config.grasp_pose.empty()) {
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
geometry_msgs::msg::TransformStamped grasp_transform_stamped;
grasp_transform_stamped.transform = createTransform(grasp_pose.pose);
grasp_transform_stamped.child_frame_id = grasp_pose.name;
grasp_transform_stamped.header.stamp = m_clock->now();
grasp_transform_stamped.header.frame_id = grasp_pose.relative_at;
tp.transforms.push_back(grasp_transform_stamped);
}
}
return tp;
}
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::getTfData(const std::string &model_name) {
tf2_msgs::msg::TFMessage tp;
if (!m_assembly_config.absolute_part.empty()) {
for (auto &abs_part : m_assembly_config.absolute_part) {
geometry_msgs::msg::TransformStamped abs_transrorm_stamped;
abs_transrorm_stamped.transform = createTransform(abs_part.pose);
abs_transrorm_stamped.child_frame_id = abs_part.name;
abs_transrorm_stamped.header.frame_id = "world";
abs_transrorm_stamped.header.stamp = m_clock->now();
tp.transforms.push_back(abs_transrorm_stamped);
}
} else {
RCLCPP_ERROR(m_logger, "Absolute parts is empty size: %zu",
m_assembly_config.absolute_part.size());
}
bool found_model = false;
bool found_grasp_pose = false;
if (!m_assembly_config.relative_part.empty()) {
const auto &absolute_part_pose = m_assembly_config.absolute_part.pose;
tp.transforms.emplace_back().transform = createTransform(absolute_part_pose);
tp.transforms.emplace_back().child_frame_id = "world";
tp.transforms.emplace_back().header.stamp = m_clock->now();
for (const auto &relative_part : m_assembly_config.relative_part) {
// Find our model data
if (relative_part.name == model_name) {
tp.transforms.emplace_back().transform = createTransform(relative_part.pose);
tp.transforms.emplace_back().child_frame_id = relative_part.relative_at;
tp.transforms.emplace_back().header.stamp = m_clock->now();
geometry_msgs::msg::TransformStamped relative_transform_stamped;
relative_transform_stamped.transform =
createTransform(relative_part.pose);
relative_transform_stamped.child_frame_id = relative_part.name;
relative_transform_stamped.header.frame_id = relative_part.relative_at;
relative_transform_stamped.header.stamp = m_clock->now();
tp.transforms.push_back(relative_transform_stamped);
found_model = true;
}
RCLCPP_INFO(m_logger, "Model name [%s]", relative_part.name.c_str());
RCLCPP_INFO(m_logger, "Model name [%s]",
relative_part.relative_at.c_str());
}
} else {
RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu", m_assembly_config.relative_part.size());
RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu",
m_assembly_config.relative_part.size());
}
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
if (grasp_pose.name == model_name) {
tp.transforms.emplace_back().transform = createTransform(grasp_pose.pose);
tp.transforms.emplace_back().child_frame_id = grasp_pose.name;
tp.transforms.emplace_back().header.stamp = m_clock->now();
found_grasp_pose = true;
if (!m_assembly_config.grasp_pose.empty()) {
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
if (grasp_pose.name == model_name) {
geometry_msgs::msg::TransformStamped grasp_transform_stamped;
grasp_transform_stamped.transform = createTransform(grasp_pose.pose);
grasp_transform_stamped.child_frame_id = grasp_pose.name + "_grasp";
grasp_transform_stamped.header.stamp = m_clock->now();
grasp_transform_stamped.header.frame_id = grasp_pose.name;
tp.transforms.push_back(grasp_transform_stamped);
found_grasp_pose = true;
}
}
}
@ -117,7 +208,8 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
RCLCPP_ERROR(m_logger, "Model %s not found in config", model_name.c_str());
}
if (!found_grasp_pose) {
RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s", model_name.c_str());
RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s",
model_name.c_str());
}
return tp;
@ -136,42 +228,41 @@ AssemblyConfigLoader::createTransform(const geometry_msgs::msg::Pose &pose) {
return transform;
}
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getWorkspace() {
tf2_msgs::msg::TFMessage tf_msg;
geometry_msgs::msg::PoseArray AssemblyConfigLoader::getWorkspace() {
geometry_msgs::msg::PoseArray pose_array;
pose_array.header.frame_id = "world";
if (m_assembly_config.workspace.empty()) {
RCLCPP_WARN(m_logger, "Workspace is empty, check your robossembler_db");
return tf_msg;
return pose_array;
}
const std::string child_frame_id = "world";
pose_array.poses.reserve(m_assembly_config.workspace.size());
const double default_rotation_value = 0.0;
const double default_rotation_w = 1.0;
tf_msg.transforms.reserve(m_assembly_config.workspace.size());
for (std::size_t i = 0; i < m_assembly_config.workspace.size(); ++i) {
tf_msg.transforms[i].transform.translation.x =
m_assembly_config.workspace[i].x;
tf_msg.transforms[i].transform.translation.y =
m_assembly_config.workspace[i].y;
tf_msg.transforms[i].transform.translation.z =
m_assembly_config.workspace[i].z;
tf_msg.transforms[i].child_frame_id = child_frame_id;
tf_msg.transforms[i].transform.rotation.x = default_rotation_value;
tf_msg.transforms[i].transform.rotation.y = default_rotation_value;
tf_msg.transforms[i].transform.rotation.z = default_rotation_value;
tf_msg.transforms[i].transform.rotation.w = default_rotation_w;
tf_msg.transforms[i].header.stamp = m_clock->now();
for (const auto &point : m_assembly_config.workspace) {
geometry_msgs::msg::Pose pose;
pose.position.x = point.x;
pose.position.y = point.y;
pose.position.z = point.z;
pose.orientation.x = default_rotation_value;
pose.orientation.y = default_rotation_value;
pose.orientation.z = default_rotation_value;
pose.orientation.w = default_rotation_w;
pose_array.poses.push_back(pose);
}
return tf_msg;
}
return pose_array;
}
geometry_msgs::msg::PoseArray
AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
geometry_msgs::msg::PoseArray pose_array;
pose_array.header.frame_id = "world";
auto workspace = getWorkspace();
for (auto &point : workspace.transforms) {
for (auto &point : workspace.poses) {
auto pose = transformTrajectory(point);
pose_array.poses.push_back(pose);
}
@ -179,13 +270,15 @@ AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
}
geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
const geometry_msgs::msg::TransformStamped &pose) {
auto pose_eigen = tf2::transformToEigen(pose.transform);
const geometry_msgs::msg::Pose &pose) {
Eigen::Isometry3d pose_eigen;
tf2::fromMsg(pose, pose_eigen);
Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitY());
pose_eigen.rotate(rotation);
pose_eigen.translation().z() += 0.35;
auto pose_msg = tf2::toMsg(pose_eigen);
return pose_msg;
return tf2::toMsg(pose_eigen);
}
} // namespace rbs_utils

5
rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg
View file

@ -1,6 +1,7 @@
string assets_db
geometry_msgs/Point[] workspace
rbs_utils_interfaces/NamedPose absolute_part
rbs_utils_interfaces/NamedPose[] absolute_part
rbs_utils_interfaces/RelativeNamedPose[] relative_part
rbs_utils_interfaces/NamedPose[] grasp_pose
rbs_utils_interfaces/RelativeNamedPose[] grasp_pose
rbs_utils_interfaces/NamedPose[] extra_poses