From 86fd3d105537c450f6c5d43630b14c5cbdf039e1 Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Tue, 19 Dec 2023 08:17:38 +0300
Subject: [PATCH] env_manager: fix target pose loader

---
 env_manager/gz_enviroment/src/gz_enviroment.cpp | 3 ---
 rbs_utils/src/rbs_utils.cpp                     | 6 +++---
 2 files changed, 3 insertions(+), 6 deletions(-)

diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp
index e75d89e..af28d15 100644
--- a/env_manager/gz_enviroment/src/gz_enviroment.cpp
+++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp
@@ -43,9 +43,6 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
   m_follow_frames = m_config_loader->getSceneModelNames();
   // m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
   m_transforms = m_config_loader->getTfData("bishop");
-  for (auto &place : m_transforms.transforms) {
-    place.child_frame_id = place.child_frame_id + "_target";
-  }
 
   // if (!doGzSpawn())
   // {
diff --git a/rbs_utils/src/rbs_utils.cpp b/rbs_utils/src/rbs_utils.cpp
index 5b1574a..6cf85c3 100644
--- a/rbs_utils/src/rbs_utils.cpp
+++ b/rbs_utils/src/rbs_utils.cpp
@@ -214,7 +214,7 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
       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_part.at("name").get<std::string>() + "_target";
       relative_transform.transform.translation.x =
           convertToDouble(relative_part.at("position").at("x"));
       relative_transform.transform.translation.y =
@@ -238,9 +238,9 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
     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_pose.at("name").get<std::string>() + "_target";
       grasp_transform.child_frame_id =
-          grasp_pose.at("name").get<std::string>() + "_grasp";
+          grasp_pose.at("name").get<std::string>() + "_target" + "_grasp";
       grasp_transform.transform.translation.x =
           convertToDouble(grasp_pose.at("position").at("x"));
       grasp_transform.transform.translation.y =