fix rbs_utils
This commit is contained in:
parent
44b927f9a9
commit
fa03887099
3 changed files with 93 additions and 80 deletions
|
@ -51,10 +51,9 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
|
|||
m_service_spawn = std::string("/world/") + m_world_name + "/create";
|
||||
m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
|
||||
"asp-example", getNode());
|
||||
m_follow_frames = m_config_loader->getSceneModelNames();
|
||||
m_transforms = m_config_loader->getTfDataAllPossible();
|
||||
m_timer = getNode()->create_wall_timer(
|
||||
100ms, std::bind(&GzEnviroment::broadcast_timer_callback, this));
|
||||
m_follow_frames = m_config_loader->getUniqueSceneModelNames();
|
||||
m_transforms = m_config_loader->getGraspTfData("bishop");
|
||||
// m_transforms = m_config_loader->getAllPossibleTfData();
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
@ -63,9 +62,9 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
|
|||
// Setting up the subscribers and publishers
|
||||
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
|
||||
|
||||
// Initialize tf broadcaster before use in subscriber
|
||||
m_tf2_broadcaster =
|
||||
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
|
||||
|
||||
m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
|
@ -121,6 +120,7 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
|
|||
}
|
||||
|
||||
for (auto &place : m_transforms.transforms) {
|
||||
place.header.stamp = getNode()->get_clock()->now();
|
||||
all_transforms.push_back(place);
|
||||
}
|
||||
|
||||
|
|
|
@ -53,11 +53,11 @@ public:
|
|||
const rclcpp::node_interfaces::NodeClockInterface::SharedPtr
|
||||
&t_clock_node_interface);
|
||||
|
||||
tf2_msgs::msg::TFMessage getAllPossibleTfData();
|
||||
tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
|
||||
tf2_msgs::msg::TFMessage getTfDataAllPossible();
|
||||
tf2_msgs::msg::TFMessage getGraspTfData(const std::string &model_name);
|
||||
|
||||
|
||||
std::vector<std::string> getSceneModelNames();
|
||||
std::vector<std::string> getUniqueSceneModelNames();
|
||||
|
||||
void printWorkspace();
|
||||
geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory();
|
||||
|
|
|
@ -55,12 +55,7 @@ void AssemblyConfigLoader::parseRbsDb(const std::string &filepath) {
|
|||
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());
|
||||
|
@ -69,7 +64,7 @@ void AssemblyConfigLoader::parseRbsDb(const std::string &filepath) {
|
|||
|
||||
[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() {}
|
||||
|
||||
std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
|
||||
std::vector<std::string> AssemblyConfigLoader::getUniqueSceneModelNames() {
|
||||
std::vector<std::string> model_names;
|
||||
if (m_assembly_config.relative_part.size() != 0) {
|
||||
for (auto &t : m_assembly_config.relative_part) {
|
||||
|
@ -86,66 +81,78 @@ std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
|
|||
return model_names;
|
||||
}
|
||||
|
||||
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAdditionalPoses() {
|
||||
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAllPossibleTfData() {
|
||||
tf2_msgs::msg::TFMessage tp;
|
||||
if (m_assembly_config.extra_poses.empty()) {
|
||||
RCLCPP_ERROR(m_logger, "Extra poses is empty");
|
||||
return tp;
|
||||
// Get absolute parts
|
||||
for (auto &abs_poses : m_assembly_config.absolute_part) {
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(abs_poses.pose);
|
||||
tmp.child_frame_id = abs_poses.name;
|
||||
tmp.header.frame_id = "world";
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
}
|
||||
|
||||
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";
|
||||
// Get relative parts
|
||||
for (const auto &relative_part : m_assembly_config.relative_part) {
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(relative_part.pose);
|
||||
tmp.child_frame_id = relative_part.name;
|
||||
tmp.header.frame_id = relative_part.relative_at;
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
RCLCPP_INFO(m_logger, "Model name [%s]", relative_part.name.c_str());
|
||||
}
|
||||
// Get grasp poses
|
||||
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(grasp_pose.pose);
|
||||
tmp.child_frame_id = grasp_pose.name;
|
||||
tmp.header.frame_id = grasp_pose.relative_at;
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
}
|
||||
|
||||
return tp;
|
||||
}
|
||||
|
||||
tf2_msgs::msg::TFMessage AssemblyConfigLoader::getTfDataAllPossible() {
|
||||
tf2_msgs::msg::TFMessage
|
||||
AssemblyConfigLoader::getGraspTfData(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);
|
||||
bool found_grasp_pose = false;
|
||||
if (!m_assembly_config.relative_part.empty()) {
|
||||
for (auto &abs_poses : m_assembly_config.absolute_part) {
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(abs_poses.pose);
|
||||
tmp.child_frame_id = abs_poses.name;
|
||||
tmp.header.frame_id = "world";
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(m_logger, "Absolute parts is empty size: %zu",
|
||||
m_assembly_config.absolute_part.size());
|
||||
RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu",
|
||||
m_assembly_config.relative_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());
|
||||
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
|
||||
if (grasp_pose.relative_at == model_name) {
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(grasp_pose.pose);
|
||||
tmp.child_frame_id = grasp_pose.name;
|
||||
tmp.header.frame_id = grasp_pose.relative_at;
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
found_grasp_pose = true;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
if (!found_grasp_pose) {
|
||||
RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s",
|
||||
model_name.c_str());
|
||||
}
|
||||
|
||||
return tp;
|
||||
|
||||
}
|
||||
|
||||
tf2_msgs::msg::TFMessage
|
||||
|
@ -169,16 +176,24 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
|
|||
bool found_model = false;
|
||||
bool found_grasp_pose = false;
|
||||
if (!m_assembly_config.relative_part.empty()) {
|
||||
for (auto &abs_poses : m_assembly_config.absolute_part) {
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(abs_poses.pose);
|
||||
tmp.child_frame_id = abs_poses.name;
|
||||
tmp.header.frame_id = "world";
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
}
|
||||
|
||||
for (const auto &relative_part : m_assembly_config.relative_part) {
|
||||
// Find our model data
|
||||
if (relative_part.name == model_name) {
|
||||
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);
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(relative_part.pose);
|
||||
tmp.child_frame_id = relative_part.name;
|
||||
tmp.header.frame_id = relative_part.relative_at;
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
found_model = true;
|
||||
}
|
||||
RCLCPP_INFO(m_logger, "Model name [%s]",
|
||||
|
@ -189,18 +204,15 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
|
|||
m_assembly_config.relative_part.size());
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
|
||||
if (grasp_pose.relative_at == model_name) {
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
tmp.transform = createTransform(grasp_pose.pose);
|
||||
tmp.child_frame_id = grasp_pose.name;
|
||||
tmp.header.frame_id = grasp_pose.relative_at;
|
||||
tmp.header.stamp = m_clock->now();
|
||||
tp.transforms.push_back(tmp);
|
||||
found_grasp_pose = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -261,17 +273,18 @@ 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.poses) {
|
||||
auto pose = transformTrajectory(point);
|
||||
pose_array.poses.push_back(pose);
|
||||
|
||||
auto workspace_poses = getWorkspace();
|
||||
|
||||
for (const auto &pose : workspace_poses.poses) {
|
||||
pose_array.poses.push_back(transformTrajectory(pose));
|
||||
}
|
||||
|
||||
return pose_array;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
|
||||
const geometry_msgs::msg::Pose &pose) {
|
||||
|
||||
Eigen::Isometry3d pose_eigen;
|
||||
tf2::fromMsg(pose, pose_eigen);
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue