add workspace base functional

Workspace functional includes basic read from workspace and trajectory calculation for inspection

env_manager/env_interface/include/env_interface/env_interface.hpp

@@ -11,6 +11,6 @@ public:
   EnvInterface() = default;
   virtual ~EnvInterface() = default;
 protected:
-  std::shared_ptr<std::vector<rbs_utils::EnvModels>> m_env_models;
+  std::shared_ptr<std::vector<rbs_utils::EnvModel>> m_env_models;
 };
 }  // namespace env_interface

env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp

@@ -1,37 +1,37 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 // #include "ros_gz_bridge/convert.hpp"
+#include "env_interface/env_interface.hpp"
+#include "gz/msgs/pose_v.pb.h"
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include "geometry_msgs/msg/pose_array.hpp"
+#include <gz/transport/Node.hh>
 #include <memory>
 #include <mutex>
 #include <string>
 #include <tf2_msgs/msg/detail/tf_message__struct.hpp>
-#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_msgs/msg/tf_message.hpp>
-#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_ros/transform_broadcaster.h>
-#include <gz/transport/Node.hh>
-#include "gz/msgs/pose_v.pb.h"
-#include "env_interface/env_interface.hpp"
 
-#include <gz/sim/components/Model.hh>
 #include <gz/sim/Entity.hh>
 #include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/components/Model.hh>
 
-namespace gz_enviroment
+namespace gz_enviroment {
-{
+using CallbackReturn =
-using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
-class GzEnviroment : public env_interface::EnvInterface
+class GzEnviroment : public env_interface::EnvInterface {
-{
 public:
   GzEnviroment();
   CallbackReturn on_init() override;
-  CallbackReturn on_configure(const rclcpp_lifecycle::State&) override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
-  CallbackReturn on_activate(const rclcpp_lifecycle::State&) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
-  CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
-  CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override;
+  CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
 
 protected:
-  void onGzPoseSub(const gz::msgs::Pose_V& pose_v);
+  void onGzPoseSub(const gz::msgs::Pose_V &pose_v);
 
   bool doGzSpawn();
 
@@ -47,9 +47,11 @@ private:
   tf2_msgs::msg::TFMessage m_transforms;
   tf2_msgs::msg::TFMessage m_target_places;
 
-
   std::string getGzWorldName();
-  std::string createGzModelString(const std::vector<double>& pose, const std::vector<double>& inertia, const double &mass, const std::string& mesh_filepath,
+  std::string createGzModelString(const std::vector<double> &pose,
-                                  const std::string& name);
+                                  const std::vector<double> &inertia,
+                                  const double &mass,
+                                  const std::string &mesh_filepath,
+                                  const std::string &name);
 };
-}  // namespace gz_enviroment
+} // namespace gz_enviroment

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -4,6 +4,7 @@
 #include "ros_gz_bridge/convert.hpp"
 #include <memory>
 #include <rclcpp/logging.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
 
 #include <gz/sim/components.hh>
@@ -25,7 +26,8 @@ CallbackReturn GzEnviroment::on_init() {
   for (const auto entity : modelEntities) {
     const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
     if (modelName) {
-      RCLCPP_INFO_STREAM(getNode()->get_logger(), "Model Name: " << modelName->Data());
+      RCLCPP_INFO_STREAM(getNode()->get_logger(),
+                         "Model Name: " << modelName->Data());
     }
   }
   return CallbackReturn::SUCCESS;
@@ -38,11 +40,13 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
   m_world_name = getGzWorldName();
   m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
   m_service_spawn = std::string("/world/") + m_world_name + "/create";
-  m_config_loader =
+  m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
-      std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2");
+      "asp-example2", getNode());
   m_follow_frames = m_config_loader->getSceneModelNames();
   // m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
   m_transforms = m_config_loader->getTfData("bishop");
+  // TODO add workspace viewer in Rviz
+  // m_config_loader->printWorkspace();
 
   // if (!doGzSpawn())
   // {
@@ -67,11 +71,7 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
 
 CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
   RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
-  // Clear tf2 broadcasters
+  // Clear variables on_cleanup
-  // m_tf2_broadcaster.reset();
-  // m_tf2_broadcaster_target.reset();
-
-  // Clear other variables
   m_gz_node.reset();
   m_follow_frames.clear();
   m_topic_name.clear();
@@ -86,10 +86,7 @@ CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
 
 CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
   RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
-  // m_tf2_broadcaster.reset();
+  // What we should use here?
-  // m_tf2_broadcaster_target.reset();
-  // m_gz_node.reset();
-
   return CallbackReturn::SUCCESS;
 }
 
@@ -133,7 +130,6 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
   }
 }
 
-
 std::string GzEnviroment::getGzWorldName() {
   bool executed{false};
   bool result{false};
@@ -212,4 +208,4 @@ std::string GzEnviroment::createGzModelString(
 
 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment,
-                       env_interface::EnvInterface);
+                       env_interface::EnvInterface);

rbs_utils/CMakeLists.txt

@@ -8,12 +8,22 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_eigen REQUIRED)
+find_package(rviz_visual_tools REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+set(deps
+  rclcpp
+  rclcpp_lifecycle
+  tf2_ros
+  tf2_eigen
+  rviz_visual_tools
+  geometry_msgs
+)
+
 
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp)
 
@@ -22,7 +32,7 @@ install(
   DESTINATION include
 )
 
-ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp tf2_ros tf2_eigen)
+ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps})
 
 # target_include_directories(asm_folder_process
 #   PUBLIC
@@ -57,4 +67,7 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+ ${deps} 
+)
 ament_package()

rbs_utils/include/rbs_utils/rbs_utils.hpp

@@ -1,24 +1,25 @@
+#include <Eigen/Core>
+#include "geometry_msgs/msg/pose.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rviz_visual_tools/rviz_visual_tools.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "tf2_ros/buffer.h"
+#include <tf2/convert.h>
 #include "tf2_ros/static_transform_broadcaster.h"
-
 #include <fstream>
-#include <memory>
 #include <nlohmann/json.hpp>
+#include <rclcpp/logger.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>
-#include <vector>
 
 const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR");
 
 namespace rbs_utils {
 
-struct EnvModels {
+struct EnvModel {
   std::string model_name;
   std::string mesh_path;
   std::vector<double> model_inertia;
@@ -26,10 +27,23 @@ struct EnvModels {
   double mass;
 };
 
-class AssemblyConfigLoader {
+struct Workspace {
+  std::string name;
+  geometry_msgs::msg::PoseArray poses;
+};
 
+class AssemblyConfigLoader {
 public:
-  AssemblyConfigLoader(const std::string &t_assembly_dir);
+  template <typename NodePtr>
+  explicit AssemblyConfigLoader(const std::string &t_assembly_dir,
+                                const NodePtr &t_node)
+      : AssemblyConfigLoader(t_assembly_dir,
+                             t_node->get_node_logging_interface()) {}
+
+  explicit AssemblyConfigLoader(
+      const std::string &t_assembly_dir,
+      const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+          &t_logger_node_interface);
 
   inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>>
   getAssemblyFileData() {
@@ -37,31 +51,40 @@ public:
   }
 
   tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
-  inline std::shared_ptr<std::vector<EnvModels>> getEnvModels() {
+  inline std::shared_ptr<std::vector<EnvModel>> getEnvModels() {
     return m_env_models;
   }
 
   std::vector<std::string> getSceneModelNames();
 
+  void printWorkspace();
+  geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory();
+  geometry_msgs::msg::Pose
+  transformTrajectory(const geometry_msgs::msg::TransformStamped &pose);
+
 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::shared_ptr<std::vector<EnvModel>> m_env_models{};
   std::string m_assembly_dir;
-
   std::unique_ptr<tf2_ros::Buffer> m_tf_buffer;
-
+  // rviz_visual_tools::RvizVisualToolsPtr m_visual_tools;
+  rclcpp::Logger m_logger;
   void readAssemblyFileData(const std::string &filename,
                             const std::string &filepath);
 
+  tf2_msgs::msg::TFMessage readWorkspaceJson(const nlohmann::json &json);
+
   tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json,
                                                 const std::string &model_name);
 
+  void publishWorkspace(const tf2_msgs::msg::TFMessage &tf_msg);
+
   void setTfFromDb(const std::string &filename);
   double convertToDouble(const nlohmann::json &value);
   void readModelConfigs();
-  EnvModels parseModelData(const std::string &jsonFilePath);
+  EnvModel parseModelData(const std::string &jsonFilePath);
 };
 
 class StaticFramePublisher : public rclcpp::Node {

rbs_utils/package.xml

@@ -8,7 +8,7 @@
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-
+  <depend>rviz_visual_tools</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

rbs_utils/src/rbs_utils.cpp

@@ -1,26 +1,24 @@
 #include "rbs_utils/rbs_utils.hpp"
-#include <fstream>
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
-#include <iostream>
+#include <tf2/LinearMath/Transform.h>
-#include <iterator>
+#include <tf2/convert.h>
-#include <memory>
+#include <tf2_eigen/tf2_eigen.hpp>
-#include <nlohmann/json_fwd.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include <ostream>
+#include <tf2_ros/buffer_interface.h>
-#include <rclcpp/clock.hpp>
-#include <rclcpp/logger.hpp>
-#include <rclcpp/logging.hpp>
-#include <string>
-#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(
-AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
+    const std::string &t_assembly_dir,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
+        &t_logging_interface)
     : m_env_vars(
           std::make_shared<std::unordered_map<std::string, nlohmann::json>>()),
-      m_env_models(std::make_shared<std::vector<EnvModels>>()),
+      m_env_models(std::make_shared<std::vector<EnvModel>>()),
-      m_assembly_dir(t_assembly_dir) {
+      m_assembly_dir(t_assembly_dir),
+      m_logger(t_logging_interface->get_logger()) {
+  // m_visual_tools.reset(new
+  // rviz_visual_tools::RvizVisualTools("world","/rviz_visual_markers",
+  // m_node));
   if (!m_assembly_dir.empty()) {
     std::vector<std::string> filenames = {"robossembler_db", "sequences"};
     for (auto &filename : filenames) {
@@ -31,9 +29,8 @@ AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
       readAssemblyFileData(filename, filepath);
     }
     readModelConfigs();
-
   } else {
-    RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Assembly dir is not set");
+    RCLCPP_ERROR(m_logger, "Assembly dir is not set");
   }
 }
 
@@ -42,44 +39,39 @@ void AssemblyConfigLoader::readAssemblyFileData(const std::string &filename,
   try {
     std::ifstream i(filepath);
     if (!i.is_open()) {
-      RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s",
+      RCLCPP_ERROR(m_logger, "Failed to open file: %s", filepath.c_str());
-                   filepath.c_str());
       return;
     }
 
     nlohmann::json json = nlohmann::json::parse(i);
     m_env_vars->insert({filename, json});
   } catch (const nlohmann::json::parse_error &e) {
-    RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
+    RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
-                 "Error parsing JSON in file %s: %s", filepath.c_str(),
+                 filepath.c_str(), e.what());
-                 e.what());
   } catch (const std::exception &e) {
-    RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
+    RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(),
-                 "Exception reading file %s: %s", filepath.c_str(), e.what());
+                 e.what());
   }
 }
 
-EnvModels
+EnvModel
 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",
+      RCLCPP_ERROR(m_logger, "Failed to open file: %s", json_file_path.c_str());
-                   json_file_path.c_str());
+      return EnvModel{};
-      return EnvModels{};
     }
     json_content = nlohmann::json::parse(file);
   } catch (const nlohmann::json::parse_error &e) {
-    RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
+    RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
-                 "Error parsing JSON in file %s: %s", json_file_path.c_str(),
+                 json_file_path.c_str(), e.what());
-                 e.what());
   } catch (const std::exception &e) {
-    RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
+    RCLCPP_ERROR(m_logger, "Exception reading file %s: %s",
-                 "Exception reading file %s: %s", json_file_path.c_str(),
+                 json_file_path.c_str(), e.what());
-                 e.what());
   }
-  EnvModels envModel;
+  EnvModel envModel;
   if (!json_content.empty()) {
     envModel.model_name = json_content.at("name").get<std::string>();
     envModel.mesh_path = env_dir + "/" + m_assembly_dir + "/" +
@@ -114,7 +106,7 @@ void AssemblyConfigLoader::readModelConfigs() {
     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());
+        EnvModel model = parseModelData(entry.path().string());
         m_env_models->push_back(model);
       }
     }
@@ -142,8 +134,7 @@ std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
       }
     }
   } else {
-    RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
+    RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
-                 "Key 'robossembler_db' not found in m_env_vars.");
     return model_names;
   }
 
@@ -165,7 +156,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
     // Output position information to console
     for (const auto &transform : tf_msg.transforms) {
       RCLCPP_INFO_STREAM(
-          rclcpp::get_logger("rbs_utils"),
+          m_logger,
           "Frame ID: " << transform.header.frame_id << ", Child Frame ID: "
                        << transform.child_frame_id << ", Translation: ["
                        << transform.transform.translation.x << ", "
@@ -175,8 +166,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
     // auto r = std::make_shared<StaticFramePublisher>(tf_msg);
 
   } else {
-    RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
+    RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
-                 "Key 'robossembler_db' not found in m_env_vars.");
   }
   return tf_msg;
 }
@@ -262,6 +252,122 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
   return tf_msg;
 }
 
+tf2_msgs::msg::TFMessage
+AssemblyConfigLoader::readWorkspaceJson(const nlohmann::json &json) {
+  tf2_msgs::msg::TFMessage tf_msg;
+  int i{0};
+  if (json.contains("workspace") && json.at("workspace").is_array()) {
+    for (auto &point : json.at("workspace")) {
+      if (point.contains("position") && point.at("position").is_object()) {
+        geometry_msgs::msg::TransformStamped workspace_point;
+        workspace_point.header.frame_id = "world";
+        workspace_point.child_frame_id = "bbox_" + std::to_string(i);
+        i++;
+
+        if (point["position"].contains("x") &&
+            point["position"].contains("y") &&
+            point["position"].contains("z")) {
+          workspace_point.transform.translation.x =
+              point["position"]["x"].get<double>();
+          workspace_point.transform.translation.y =
+              point["position"]["y"].get<double>();
+          workspace_point.transform.translation.z =
+              point["position"]["z"].get<double>();
+          tf_msg.transforms.push_back(workspace_point);
+        } else {
+          RCLCPP_ERROR(m_logger,
+                       "Cannot find key 'x', 'y', 'z' in 'position' or 'name' "
+                       "isn't found in key 'workspace'");
+        }
+      } else {
+        RCLCPP_ERROR(m_logger,
+                     "Cannot find key 'position', 'name', or 'position' isn't "
+                     "an object in key 'workspace'");
+      }
+    }
+  } else {
+    RCLCPP_ERROR(m_logger, "Cannot find key 'workspace' in robossembler_db");
+  }
+
+  return tf_msg;
+}
+
+void AssemblyConfigLoader::publishWorkspace(
+    const tf2_msgs::msg::TFMessage &tf_msg) {
+  // if (tf_msg.transforms.size() >= 4) {
+  //   auto pose1 =
+  //   Eigen::Vector3d(tf_msg.transforms[0].transform.translation.x,
+  //                                tf_msg.transforms[0].transform.translation.y,
+  //                                tf_msg.transforms[0].transform.translation.z);
+  //
+  //   auto pose2 =
+  //   Eigen::Vector3d(tf_msg.transforms[1].transform.translation.x,
+  //                                tf_msg.transforms[1].transform.translation.y,
+  //                                tf_msg.transforms[1].transform.translation.z);
+  //
+  //   auto pose3 =
+  //   Eigen::Vector3d(tf_msg.transforms[2].transform.translation.x,
+  //                                tf_msg.transforms[2].transform.translation.y,
+  //                                tf_msg.transforms[2].transform.translation.z);
+  //
+  //   auto pose4 =
+  //   Eigen::Vector3d(tf_msg.transforms[3].transform.translation.x,
+  //                                tf_msg.transforms[3].transform.translation.y,
+  //                                tf_msg.transforms[3].transform.translation.z);
+  //
+  //   // Connect workspace points as box
+  //   m_visual_tools->publishLine(pose1, pose2, rviz_visual_tools::GREEN);
+  //   m_visual_tools->publishLine(pose2, pose3, rviz_visual_tools::GREEN);
+  //   m_visual_tools->publishLine(pose3, pose4, rviz_visual_tools::GREEN);
+  //   m_visual_tools->publishLine(pose4, pose1, rviz_visual_tools::GREEN);
+  // } else {
+  //   RCLCPP_ERROR(m_logger,
+  //                "Not enough points to form a square");
+  // }
+}
+
+void AssemblyConfigLoader::printWorkspace() {
+  if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
+    const nlohmann::json json = (*m_env_vars)["robossembler_db"];
+    if (json.contains("workspace")) {
+      auto workspace = readWorkspaceJson(json);
+      publishWorkspace(workspace);
+    } else {
+      RCLCPP_WARN(m_logger, "Workspace isn't set in robossembler_db");
+    }
+  } else {
+    RCLCPP_ERROR(m_logger, "Cannot find robossembler_db");
+  }
+}
+
+geometry_msgs::msg::PoseArray
+AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
+  geometry_msgs::msg::PoseArray pose_array;
+  pose_array.header.frame_id = "world";
+  if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
+    const nlohmann::json json = (*m_env_vars)["robossembler_db"];
+    if (json.contains("workspace")) {
+      auto workspace = readWorkspaceJson(json);
+      for (auto &point : workspace.transforms) {
+        geometry_msgs::msg::Pose pose;
+        pose = transformTrajectory(point);
+        pose_array.poses.push_back(pose);
+      }
+    }
+  }
+  return pose_array;
+}
+
+geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
+    const geometry_msgs::msg::TransformStamped &pose) {
+  auto pose_eigen = tf2::transformToEigen(pose.transform);
+  Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitX());
+  pose_eigen.rotate(rotation);
+  pose_eigen.translation().z() += 0.5;
+  auto pose_msg = tf2::toMsg(pose_eigen);
+  return pose_msg;
+}
+
 double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
   if (value.is_number()) {
     return value.get<double>();
@@ -269,8 +375,7 @@ double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
     try {
       return std::stod(value.get<std::string>());
     } catch (const std::exception &e) {
-      RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
+      RCLCPP_ERROR(m_logger, "Error converting string to double: %s", e.what());
-                   "Error converting string to double: %s", e.what());
       throw std::runtime_error("Error converting string to double");
     }
   }
@@ -278,4 +383,4 @@ double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
   throw std::runtime_error("Invalid JSON type for conversion to double");
 }
 
-} // namespace rbs_utils
+} // namespace rbs_utils