rbs_utils in env_manager and clear code

Dockerfile

@@ -3,6 +3,8 @@ FROM osrf/ros:humble-desktop
 ARG WSDIR=rbs_ws
 
 ENV RBS_ASSEMBLY_DIR=/assembly
+
+# COPY /home/bill-finger/assembly /assembly
 ENV IGN_GAZEBO_RESOURCE_PATH=/${WSDIR}/install/rbs_simulation/share/rbs_simulation/
 
 RUN apt update && apt install -y \ 

env_manager/env_interface/CMakeLists.txt

@@ -7,6 +7,9 @@ endif()
 
 set(INCLUDE_DEPENDS
   rclcpp_lifecycle
+  rbs_utils
+  tf2_ros
+  tf2_eigen
 )
 
 # find dependencies

env_manager/env_interface/include/env_interface/env_interface.hpp

@@ -1,4 +1,7 @@
 #include "env_interface/env_interface_base.hpp"
+#include "rbs_utils/rbs_utils.hpp"
+#include <memory>
+#include <vector>
 
 namespace env_interface
 {
@@ -7,5 +10,7 @@ class EnvInterface : public EnvInterfaceBase
 public:
   EnvInterface() = default;
   virtual ~EnvInterface() = default;
+protected:
+  std::shared_ptr<std::vector<rbs_utils::EnvModels>> m_env_models;
 };
 }  // namespace env_interface

env_manager/env_interface/package.xml

@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rbs_utils</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

env_manager/env_interface/src/env_interface.cpp

@@ -0,0 +1,6 @@
+#include "env_interface/env_interface.hpp"
+
+namespace env_interface {
+
+
+}

env_manager/env_manager/include/env_manager/env_manager.hpp

@@ -1,51 +1,53 @@
 #ifndef __ENV_MANAGER_HPP__
 #define __ENV_MANAGER_HPP__
 
+#include "env_manager_interfaces/srv/configure_env.hpp"
+#include "env_manager_interfaces/srv/load_env.hpp"
+#include "env_manager_interfaces/srv/unload_env.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "env_manager_interfaces/srv/load_env.hpp"
-#include "env_manager_interfaces/srv/configure_env.hpp"
-#include "env_manager_interfaces/srv/unload_env.hpp"
 
 #include "env_interface/env_interface.hpp"
 
 #include "pluginlib/class_loader.hpp"
 
-namespace env_manager
+namespace env_manager {
-{
 using EnvInterface = env_interface::EnvInterface;
 using EnvInterfaceSharedPtr = std::shared_ptr<env_interface::EnvInterface>;
-using EnvStateReturnType = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+using EnvStateReturnType =
+    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
-struct EnvSpec
+struct EnvSpec {
-{
   std::string name;
   std::string type;
   EnvInterfaceSharedPtr env_ptr;
 };
 
-class EnvManager : public rclcpp::Node
+class EnvManager : public rclcpp::Node {
-{
 public:
   // EnvManager(rclcpp::Executor::SharedPtr executor,
-  //             const std::string & json_config_path,
+  //            const std::string &asm_config,
-  //             const std::string & node_name = "env_manager",
+  //            const std::string &node_name = "env_manager",
-  //             const std::string & node_namespace = "",
+  //            const std::string &node_namespace = "",
-  //             rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()
+  //            rclcpp::NodeOptions &node_options =
+  //                rclcpp::NodeOptions()
   //                    .allow_undeclared_parameters(true)
   //                    .automatically_declare_parameters_from_overrides(true));
-  EnvManager(rclcpp::Executor::SharedPtr executor, const std::string& node_name = "env_manager",
+  EnvManager(rclcpp::Executor::SharedPtr executor,
-             const std::string& node_namespace = "",
+             const std::string &node_name = "env_manager",
-             rclcpp::NodeOptions& node_options = rclcpp::NodeOptions()
+             const std::string &node_namespace = "",
+             rclcpp::NodeOptions &node_options =
+                 rclcpp::NodeOptions()
                      .allow_undeclared_parameters(true)
                      .automatically_declare_parameters_from_overrides(true));
   virtual ~EnvManager() = default;
 
   // EnvInterfaceSharedPtr loadEnv(const std::string& env_name);
-  EnvInterfaceSharedPtr loadEnv(const std::string& env_name, const std::string& env_class_type);
+  EnvInterfaceSharedPtr loadEnv(const std::string &env_name,
-  EnvStateReturnType unloadEnv(const std::string& env_name);
+                                const std::string &env_class_type);
-  EnvStateReturnType configureEnv(const std::string& env_name);
+  EnvStateReturnType unloadEnv(const std::string &env_name);
-  EnvInterfaceSharedPtr addEnv(const EnvSpec& enviment);
+  EnvStateReturnType configureEnv(const std::string &env_name);
+  EnvInterfaceSharedPtr addEnv(const EnvSpec &enviment);
 
   // TODO: Define the input data format
   //  Set Target for RL enviroment
@@ -60,22 +62,29 @@ public:
 protected:
   void initServices();
   rclcpp::Node::SharedPtr getNode();
-  void loadEnv_cb(const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
+  void loadEnv_cb(
+      const env_manager_interfaces::srv::LoadEnv::Request::SharedPtr request,
       env_manager_interfaces::srv::LoadEnv_Response::SharedPtr response);
 
-  void configureEnv_cb(const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr request,
+  void configureEnv_cb(
+      const env_manager_interfaces::srv::ConfigureEnv::Request::SharedPtr
+          request,
       env_manager_interfaces::srv::ConfigureEnv_Response::SharedPtr response);
 
-  void unloadEnv_cb(const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
+  void unloadEnv_cb(
+      const env_manager_interfaces::srv::UnloadEnv::Request::SharedPtr request,
       env_manager_interfaces::srv::UnloadEnv::Response::SharedPtr response);
 
 private:
   rclcpp::Node::SharedPtr m_node;
   std::mutex m_env_mutex;
   std::vector<EnvSpec> m_active_envs;
-  rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr m_load_env_srv;
+  rclcpp::Service<env_manager_interfaces::srv::LoadEnv>::SharedPtr
-  rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr m_configure_env_srv;
+      m_load_env_srv;
-  rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr m_unload_env_srv;
+  rclcpp::Service<env_manager_interfaces::srv::ConfigureEnv>::SharedPtr
+      m_configure_env_srv;
+  rclcpp::Service<env_manager_interfaces::srv::UnloadEnv>::SharedPtr
+      m_unload_env_srv;
 
   rclcpp::CallbackGroup::SharedPtr m_callback_group;
   rclcpp::Executor::SharedPtr m_executor;

env_manager/env_manager/src/env_manager.cpp

@@ -1,5 +1,6 @@
 #include "env_manager/env_manager.hpp"
 #include "nlohmann/json.hpp"
+#include <string>
 
 static constexpr const char *ENV_INTERFACE_BASE_CLASS_NAMESPACE =
     "env_interface";
@@ -19,17 +20,16 @@ EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
   initServices();
 }
 
-// EnvManager::EnvManager(
+// EnvManager::EnvManager(rclcpp::Executor::SharedPtr executor,
-//     rclcpp::Executor::SharedPtr executor,
+//                        const std::string &asm_config,
-//     const std::string & json_config_path,
+//                        const std::string &node_name,
-//     const std::string & node_name,
+//                        const std::string &node_namespace,
-//     const std::string & node_namespace,
+//                        rclcpp::NodeOptions &options)
-//     rclcpp::NodeOptions & options)
+//     : rclcpp::Node(node_name, node_namespace, options), m_executor(executor),
-//     : rclcpp::Node(node_name, node_namespace, options),
+//       m_loader(
-//       m_executor(executor),
+//           std::make_shared<pluginlib::ClassLoader<env_interface::EnvInterface>>(
-//       m_loader(std::make_shared<pluginlib::ClassLoader<rclcpp_lifecycle::LifecycleNode>>(
+//               ENV_INTERFACE_BASE_CLASS_NAMESPACE,
-//           ENV_INTERFACE_BASE_CLASS_NAMESPACE, ENV_INTERFACE_BASE_CLASS_NAME))
+//               ENV_INTERFACE_BASE_CLASS_NAME)) {
-// {
 //   initServices();
 // }
 
@@ -128,6 +128,7 @@ EnvStateReturnType EnvManager::unloadEnv(const std::string &env_name) {
       [&env_name](const EnvSpec &env) { return env.name == env_name; });
 
   if (it != m_active_envs.end()) {
+    it->env_ptr->getNode()->cleanup();
     m_executor->remove_node(it->env_ptr->getNode()->get_node_base_interface());
     m_active_envs.erase(it);
     return EnvStateReturnType::SUCCESS;
@@ -162,8 +163,6 @@ EnvStateReturnType EnvManager::configureEnv(const std::string &env_name) {
     return EnvStateReturnType::FAILURE;
   }
   it->env_ptr->configure();
-  // it->env_ptr->getNode()->configure();
-  // it->env_ptr->activate();
   it->env_ptr->getNode()->activate();
 
   return EnvStateReturnType::SUCCESS;

env_manager/gz_enviroment/CMakeLists.txt

@@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     rclcpp_lifecycle
     tf2_ros
     tf2_msgs
+    tf2_eigen
     ros_gz
     pluginlib
     ignition-transport11
@@ -67,5 +68,5 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
-ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+# ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp

@@ -1,7 +1,10 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 // #include "ros_gz_bridge/convert.hpp"
+#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>
@@ -34,14 +37,19 @@ protected:
 
 private:
   std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster_target;
   std::shared_ptr<gz::transport::Node> m_gz_node;
   std::vector<std::string> m_follow_frames;
   std::string m_topic_name;
   std::string m_service_spawn;
   std::string m_world_name;
+  std::shared_ptr<rbs_utils::AssemblyConfigLoader> m_config_loader;
+  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::string& mesh_filepath,
+  std::string createGzModelString(const std::vector<double>& pose, const std::vector<double>& inertia, const double &mass, const std::string& mesh_filepath,
                                   const std::string& name);
 };
 }  // namespace gz_enviroment

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -1,49 +1,51 @@
 #include "gz_enviroment/gz_enviroment.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
-#include "ros_gz_bridge/convert.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "ros_gz_bridge/convert.hpp"
 #include <memory>
 #include <rclcpp/logging.hpp>
 #include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
 
 #include <gz/sim/components.hh>
 #include <string>
+#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
+#include <tf2_ros/transform_broadcaster.h>
 
-namespace gz_enviroment
+namespace gz_enviroment {
-{
 
-GzEnviroment::GzEnviroment() : env_interface::EnvInterface()
+GzEnviroment::GzEnviroment() : env_interface::EnvInterface() {}
-{
-}
 
-CallbackReturn GzEnviroment::on_init()
+CallbackReturn GzEnviroment::on_init() {
-{
   RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment in on_init()");
   gz::sim::EntityComponentManager ecm;
 
-  // Получение всех сущностей, которые представляют модели
+  const auto modelEntities =
-  const auto modelEntities = ecm.EntitiesByComponents(gz::sim::components::Model());
+      ecm.EntitiesByComponents(gz::sim::components::Model());
 
-  // Вывод списка моделей
+  for (const auto entity : modelEntities) {
-  for (const auto entity : modelEntities)
-  {
     const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
-    if (modelName)
+    if (modelName) {
-    {
+      RCLCPP_INFO_STREAM(getNode()->get_logger(), "Model Name: " << modelName->Data());
-      std::cout << "Model Name: " << modelName->Data() << std::endl;
     }
   }
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State&)
+CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
-{
   // Configuration of parameters
   RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
   m_gz_node = std::make_shared<gz::transport::Node>();
   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 =
+      std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2");
+  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())
   // {
@@ -53,49 +55,53 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State&)
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State&)
+CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
-{
   // Setting up the subscribers and publishers
   RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
 
   m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
-  m_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
+  m_tf2_broadcaster =
+      std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
+  m_tf2_broadcaster_target =
+      std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
 
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State&)
+CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
-{
   RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
 
   m_tf2_broadcaster.reset();
+  m_tf2_broadcaster_target.reset();
   m_gz_node.reset();
 
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn GzEnviroment::on_deactivate(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();
+  // m_tf2_broadcaster_target.reset();
+  // m_gz_node.reset();
 
   return CallbackReturn::SUCCESS;
 }
 
-// TODO: Check do this via EntityComponentManager
+// TODO: Check to do this via EntityComponentManager
-void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
+void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
-{
   // TODO: Read from config
-  m_follow_frames = { "box1", "box2", "box3", "box4", "box5", "box6"};
+  // m_follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
-  for (const auto& it : pose_v.pose())
+  std::vector<geometry_msgs::msg::TransformStamped> all_transforms{};
-  {
+
-    if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) == m_follow_frames.end())
+  for (const auto &it : pose_v.pose()) {
+    if (std::find(m_follow_frames.begin(), m_follow_frames.end(), it.name()) ==
+        m_follow_frames.end())
       continue;
 
     geometry_msgs::msg::PoseStamped rpose;
     ros_gz_bridge::convert_gz_to_ros(it, rpose);
 
     geometry_msgs::msg::TransformStamped t;
-
     t.header.stamp = getNode()->get_clock()->now();
     t.header.frame_id = "world";
     t.child_frame_id = it.name();
@@ -109,32 +115,37 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V& pose_v)
     t.transform.rotation.z = rpose.pose.orientation.z;
     t.transform.rotation.w = rpose.pose.orientation.w;
 
-    m_tf2_broadcaster->sendTransform(t);
+    all_transforms.push_back(t);
+  }
+
+  for (auto &place : m_transforms.transforms) {
+    all_transforms.push_back(place);
+  }
+
+  for (auto &transform : all_transforms) {
+    m_tf2_broadcaster->sendTransform(transform);
   }
 }
 
-std::string GzEnviroment::getGzWorldName()
+
-{
+std::string GzEnviroment::getGzWorldName() {
-  bool executed{ false };
+  bool executed{false};
-  bool result{ false };
+  bool result{false};
-  unsigned int timeout{ 5000 };
+  unsigned int timeout{5000};
-  std::string service{ "/gazebo/worlds" };
+  std::string service{"/gazebo/worlds"};
   gz::msgs::StringMsg_V worlds_msg;
 
-  while (rclcpp::ok() && !executed)
+  while (rclcpp::ok() && !executed) {
-  {
     RCLCPP_INFO(getNode()->get_logger(), "Requesting list of world names.");
     executed = m_gz_node->Request(service, timeout, worlds_msg, result);
   }
 
-  if (!executed)
+  if (!executed) {
-  {
     RCLCPP_INFO(getNode()->get_logger(), "Timed out when getting world names.");
     return "";
   }
 
-  if (!result || worlds_msg.data().empty())
+  if (!result || worlds_msg.data().empty()) {
-  {
     RCLCPP_INFO(getNode()->get_logger(), "Failed to get world names.");
     return "";
   }
@@ -142,39 +153,57 @@ std::string GzEnviroment::getGzWorldName()
   return worlds_msg.data(0);
 }
 
-bool GzEnviroment::doGzSpawn()
+bool GzEnviroment::doGzSpawn() {
-{
   gz::msgs::EntityFactory req;
   gz::msgs::Boolean rep;
   bool result;
   unsigned int timeout = 5000;
-  // TODO: From Config
+  bool executed;
-  std::vector<double> pps{ 1.0, 0.0, 1.0, 0.0, 0.0 };
-  std::string mesh_file = "monkey.stl";
-  std::string mesh_name = "monkey";
-  // ENDTODO
-  std::string sdf_string = createGzModelString(pps, mesh_file, mesh_name);
-  req.set_sdf(sdf_string);
-  bool executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
 
+  auto env_models = m_config_loader->getEnvModels();
+  for (auto &model : *env_models) {
+    std::string sdf_string =
+        createGzModelString(model.model_pose, model.model_inertia, model.mass,
+                            model.mesh_path, model.model_name);
+    req.set_sdf(sdf_string);
+    executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
+  }
   return executed;
 }
 
-std::string GzEnviroment::createGzModelString(const std::vector<double>& pose, const std::string& mesh_filepath,
+std::string GzEnviroment::createGzModelString(
-                                              const std::string& name)
+    const std::vector<double> &pose, const std::vector<double> &inertia,
-{
+    const double &mass, const std::string &mesh_filepath,
-  std::string model_template = std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
+    const std::string &name) {
-                               "<link name='link_" + name + "'>" + "<pose>" + std::to_string(pose[0]) + " " +
+  std::string model_template =
-                               std::to_string(pose[1]) + " " + std::to_string(pose[2]) + " " + std::to_string(pose[3]) +
+      std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
-                               " " + std::to_string(pose[4]) + " " + std::to_string(pose[5]) + "</pose>" +
+      "<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
-                               "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" + mesh_filepath +
+      std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
-                               "</uri></mesh></geometry>" + "</visual>" + "<collision name='collision_" + name + "'>" +
+      std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
-                               "<geometry><mesh><uri>file://" + mesh_filepath + "</uri></mesh></geometry>" +
+      std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
-                               "</collision>" + "</link>" + "</model>" + "</sdf>";
+      "</pose>"
+      // + "<inertial>"
+      // + "<inertia>"
+      // + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
+      // + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
+      // + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
+      // + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
+      // + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
+      // + "<izz>" + std::to_string(inertia[5]) + "</izz>"
+      // + "</inertia>"
+      // + "<mass>" + std::to_string(mass) + "</mass>"
+      // + "</inertial>"
+      + "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
+      mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
+      "<collision name='collision_" + name + "'>" +
+      "<geometry><mesh><uri>file://" + mesh_filepath +
+      "</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
+      "</sdf>";
   return model_template;
 }
 
 } // namespace gz_enviroment
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, env_interface::EnvInterface);
+PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment,
+                       env_interface::EnvInterface);

rbs_bt_executor/bt_trees/test_gripper.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="MainTree">
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
+            <Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
+            <Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
+            <Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
+            <Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
+            <Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
+
+        </Sequence>
+    </BehaviorTree>
+</root>

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -51,7 +51,7 @@ def generate_launch_description():
             executable='bt_engine',
             #prefix=['xterm -e gdb -ex run --args'],
             parameters=[
-                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')},
+                {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_gripper.xml')},
                 {'plugins':["rbs_skill_move_topose_bt_action_client",
                             "rbs_skill_get_pick_place_pose_service_client",
                             "rbs_skill_gripper_move_bt_action_client",

rbs_bt_executor/src/MoveToJointStates.cpp

@@ -20,7 +20,7 @@ public:
 
   MoveToJointStateAction::Goal populate_goal() override {
     getInput<std::string>("robot_name", robot_name_);
-    getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
+    getInput<std::vector<double>>("JointState", joint_values_);
 
     auto goal = MoveToJointStateAction::Goal();
     RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]",
@@ -42,7 +42,7 @@ public:
   static PortsList providedPorts() {
     return providedBasicPorts(
         {InputPort<std::string>("robot_name"),
-         InputPort<std::vector<double>>("PrePlaceJointState")});
+         InputPort<std::vector<double>>("JointState")});
   }
 
 private:

rbs_bt_executor/src/gripper_move.cpp

@@ -19,9 +19,6 @@ public:
     if (!_node->has_parameter("close")) {
       _node->declare_parameter("close", position.close);
     }
-    if (!_node->has_parameter("midPosition")) {
-      _node->declare_parameter("midPosition", position.closeFull);
-    }
   }
   GripperCommand::Goal populate_goal() override {
     getInput<std::string>("pose", pose);
@@ -44,9 +41,8 @@ public:
 
 private:
   struct {
-    double open = 0.2;
+    double open = 0.008;
-    double closeFull = 0.8;
+    double close = 0.000;
-    double close = 0.4;
   } position;
 
   std::string pose;

rbs_perception/src/perception_filter.cpp

@@ -5,7 +5,7 @@ rbs_perception::PCFilter::PCFilter() : Node("pc_filter", rclcpp::NodeOptions())
 {
     publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("rgbd_camera/filtered_points", 1);
     subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-        "rgbd_camera/points", 1, std::bind(&PCFilter::sub_callback, this, std::placeholders::_1)
+        "inner_rgbd_camera/points", 1, std::bind(&PCFilter::sub_callback, this, std::placeholders::_1)
     );
     tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
@@ -16,7 +16,7 @@ void rbs_perception::PCFilter::sub_callback(const sensor_msgs::msg::PointCloud2
 {
     try
     {
-        standform = tf_buffer_->lookupTransform(msg.header.frame_id, world_frame,
+        standform = tf_buffer_->lookupTransform(world_frame, "inner_rgbd_camera",
                                                     tf2::TimePointZero, tf2::durationFromSec(3));
 
     }
@@ -25,13 +25,13 @@ void rbs_perception::PCFilter::sub_callback(const sensor_msgs::msg::PointCloud2
         RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
     }
     sensor_msgs::msg::PointCloud2 transformed_cloud;
-    pcl_ros::transformPointCloud(world_frame, standform, msg, transformed_cloud);
+    pcl_ros::transformPointCloud("inner_rgbd_camera", standform, msg, transformed_cloud);
     pcl::PointCloud<pcl::PointXYZ> cloud;
     pcl::fromROSMsg(transformed_cloud, cloud);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>(cloud));
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>());
     voxel_filter.setInputCloud(cloud_ptr);
-    voxel_filter.setLeafSize(0.02, 0.02, 0.02);
+    voxel_filter.setLeafSize(0.01, 0.01, 0.01);
     voxel_filter.filter(*cloud_voxel_filtered);
     pub_callback(publisher_, *cloud_voxel_filtered);
 }

rbs_simulation/launch/simulation.launch.py

@@ -34,8 +34,9 @@ def generate_launch_description():
     env_manager_cond = LaunchConfiguration("env_manager")
     gazebo_gui = LaunchConfiguration("gazebo_gui")
     # FIXME: To args when we'll have different files
+    print(os.getenv("IGN_GAZEBO_RESOURCE_PATH").__str__())
     world_config_file = PathJoinSubstitution(
-        [FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"]
+        [FindPackageShare("rbs_simulation"), "worlds", "asm2.sdf"]
     )
     # Gazebo nodes
 
@@ -81,6 +82,18 @@ def generate_launch_description():
         output='screen',
     )
     
+    rgbd_bridge_in = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=[
+            '/inner_rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
+            '/inner_rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
+            '/inner_rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
+            '/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
+        ],
+        output='screen',
+    )
+    
     
 
     nodes_to_start = [
@@ -88,6 +101,7 @@ def generate_launch_description():
         gazebo_server,
         gazebo_spawn_robot,
         env_manager,
-        rgbd_bridge_out
+        rgbd_bridge_out,
+        rgbd_bridge_in
     ]
     return LaunchDescription(declared_arguments + nodes_to_start)

rbs_simulation/worlds/asm2.sdf

@@ -0,0 +1,107 @@
+<?xml version="1.0"?>
+<sdf version='1.9'>
+  <world name='asm2'>
+    <physics name='1ms' type='ignored'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <plugin name='ignition::gazebo::systems::Physics' filename='ignition-gazebo-physics-system'/>
+    <plugin name='ignition::gazebo::systems::UserCommands' filename='ignition-gazebo-user-commands-system'/>
+    <plugin name='ignition::gazebo::systems::SceneBroadcaster' filename='ignition-gazebo-scene-broadcaster-system'/>
+    <plugin name='ignition::gazebo::systems::Contact' filename='ignition-gazebo-contact-system'/>
+    <plugin name="ignition::gazebo::systems::Sensors" filename="ignition-gazebo-sensors-system">
+        <render_engine>ogre2</render_engine>
+      </plugin>  
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>false</shadows>
+    </scene>
+    <gui fullscreen="0">
+        <plugin filename="GzScene3D" name="3D View">
+          <ignition-gui>
+            <title>3D View</title>
+            <property type="bool" key="showTitleBar">false</property>
+            <property type="string" key="state">docked</property>
+          </ignition-gui>
+          <engine>ogre2</engine>
+          <scene>scene</scene>
+          <ambient_light>1.0 1.0 1.0</ambient_light>
+          <background_color>0.4 0.6 1.0</background_color>
+          <camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
+        </plugin>
+        <plugin filename="WorldStats" name="World stats">
+        <ignition-gui>
+            <title>World stats</title>
+            <property type="bool" key="showTitleBar">false</property>
+            <property type="bool" key="resizable">false</property>
+            <property type="double" key="height">110</property>
+            <property type="double" key="width">290</property>
+            <property type="double" key="z">1</property>
+            <property type="string" key="state">floating</property>
+            <anchors target="3D View">
+            <line own="right" target="right"/>
+            <line own="bottom" target="bottom"/>
+            </anchors>
+        </ignition-gui>
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+        </plugin>
+      </gui> 
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <model name='ground'> 
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+    <!-- Manipulating objects -->
+			<include>
+				<name>board</name>
+				<uri>model://board</uri>
+        <pose>0.45 0.0 0.0 0.0 0.0 0.0</pose>
+			</include>
+			<include>
+				<name>bishop</name>
+				<uri>model://bishop</uri>
+        <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose>
+			</include>
+  </world>
+</sdf>

rbs_skill_servers/src/gripper_control_action_server.cpp

@@ -32,36 +32,24 @@ public:
                   std::placeholders::_1));
     publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
         "/gripper_controller/commands", 10);
-    join_state_subscriber =
-        this->create_subscription<sensor_msgs::msg::JointState>(
-            "/joint_states", 10,
-            std::bind(&GripperControlActionServer::joint_state_callback, this,
-                      std::placeholders::_1));
   }
 
 private:
   rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
   rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
-  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr
-      join_state_subscriber;
-  double gripper_joint_state{1.0};
 
   using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
 
-  void joint_state_callback(const sensor_msgs::msg::JointState &msg) {
-    gripper_joint_state = msg.position.back();
-  }
-
   rclcpp_action::GoalResponse
   goal_callback(const rclcpp_action::GoalUUID &uuid,
                 std::shared_ptr<const GripperCommand::Goal> goal) {
 
     RCLCPP_INFO(this->get_logger(),
-                "goal request recieved for gripper [%.2f m]", goal->position);
+                "goal request recieved for gripper [%.6f m]", goal->position);
     (void)uuid;
-    if (goal->position > 0.9 or goal->position < 0) {
+    // if (goal->position > 0.008 or goal->position < 0) {
-      return rclcpp_action::GoalResponse::REJECT;
+    //   return rclcpp_action::GoalResponse::REJECT;
-    }
+    // }
     return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
   }
 
@@ -91,8 +79,6 @@ private:
       RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
       return;
     }
-    RCLCPP_INFO(this->get_logger(), "Current gripper state %f",
-                gripper_joint_state);
 
     std_msgs::msg::Float64MultiArray command;
 

rbs_skill_servers/src/pick_place_pose_loader.cpp

@@ -38,11 +38,6 @@ void GetGraspPlacePoseServer::handleServer(
         response) {
   std::string object_name =
       request->object_name + "_place"; // TODO: replace with better name
-  // Load place pose from TF2
-
-  auto d = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example");
-  d->setTfData();
-  // rbs_utils::processAsmFolderName("asp-example");
 
   try {
     place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
@@ -66,10 +61,7 @@ void GetGraspPlacePoseServer::handleServer(
                grasp_pose_tf.transform.translation.x,
                grasp_pose_tf.transform.translation.y,
                grasp_pose_tf.transform.translation.z);
-  // TODO: Here need to check the parameter
+
-  // We can use another parameter library from PickNik
-  // grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
-  //                        .as_double_array();
   RCLCPP_DEBUG(this->get_logger(),
                "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
                request->grasp_direction.x, request->grasp_direction.y,
@@ -84,8 +76,7 @@ void GetGraspPlacePoseServer::handleServer(
 
   Eigen::Vector3d scale_grasp(0, 0, 0.10);
   Eigen::Vector3d scale_place(0, 0, 0.15);
-  auto path = std::getenv("RBS_ASSEMBLY_PATH");
+  
-  RCLCPP_ERROR_STREAM(this->get_logger(), path);
   response->grasp =
       collectPose(grasp_pose, request->grasp_direction, scale_grasp);
   response->place =
@@ -129,12 +120,3 @@ std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
 
   return poses;
 }
-
-// std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::getPlacePoseJson(const nlohmann::json& json)
-// {
-//   std::vector<geometry_msgs::msg::Pose> place_pose;
-//   auto env_path = std::getenv("PATH");
-  
-//   RCLCPP_INFO_STREAM(this->get_logger(), env_path);
-//   return place_pose;
-// }

rbs_utils/CMakeLists.txt

@@ -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()

rbs_utils/include/rbs_utils/rbs_utils.hpp

@@ -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 {

rbs_utils/src/rbs_utils.cpp

@@ -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>>()) {
+          std::make_shared<std::unordered_map<std::string, nlohmann::json>>()),
-  if (!t_assembly_dir.empty()) {
+      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>>
+EnvModels
-AssemblyConfigLoader::getAssemblyFileData() {
+AssemblyConfigLoader::parseModelData(const std::string &json_file_path) {
-  return m_env_vars;
+  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,37 +172,47 @@ 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>();
+      json.at("absolutePart").at("name").get<std::string>();
-  absolute_transform.transform.translation.x = -1.0;
+  absolute_transform.transform.translation.x =
-  absolute_transform.transform.translation.y = 0.0;
+      convertToDouble(json.at("absolutePart").at("position").at("x"));
-  absolute_transform.transform.translation.z = 0.0;
+  absolute_transform.transform.translation.y =
-  absolute_transform.transform.rotation.w = 1.0;
+      convertToDouble(json.at("absolutePart").at("position").at("y"));
-  absolute_transform.transform.rotation.x = 0.0;
+  absolute_transform.transform.translation.z =
-  absolute_transform.transform.rotation.y = 0.0;
+      convertToDouble(json.at("absolutePart").at("position").at("z"));
-  absolute_transform.transform.rotation.z = 0.0;
+  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")) {
+    if (relative_part.at("name").get<std::string>() == model_name) {
       geometry_msgs::msg::TransformStamped relative_transform;
       relative_transform.header.frame_id =
-        json.at("absolutePart").get<std::string>();
+          relative_part.at("relativeAt").get<std::string>();
       relative_transform.child_frame_id =
           relative_part.at("name").get<std::string>();
       relative_transform.transform.translation.x =
@@ -119,13 +231,16 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json) {
           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")) {
+    if (grasp_pose.at("name").get<std::string>() == model_name) {
       geometry_msgs::msg::TransformStamped grasp_transform;
       grasp_transform.header.frame_id =
-        json.at("absolutePart").get<std::string>();
+          grasp_pose.at("name").get<std::string>();
-    grasp_transform.child_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 =
@@ -142,6 +257,7 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json) {
           convertToDouble(grasp_pose.at("quaternion").at("qz"));
       tf_msg.transforms.push_back(grasp_transform);
     }
+  }
 
   return tf_msg;
 }