update main

env_manager/env_manager/include/env_manager/env_manager.hpp

@@ -97,4 +97,4 @@ private:
   std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
 };
 } // namespace env_manager
-#endif // __ENV_MANAGER_HPP__
+#endif // __ENV_MANAGER_HPP__

env_manager/gz_enviroment/CMakeLists.txt

@@ -67,6 +67,12 @@ install(TARGETS ${PROJECT_NAME}
     LIBRARY DESTINATION lib
 )
 
+install(PROGRAMS
+  scripts/publish_asm_config.py
+  scripts/test_tf.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 # ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp

@@ -2,12 +2,13 @@
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 // #include "ros_gz_bridge/convert.hpp"
 #include "env_interface/env_interface.hpp"
+#include "geometry_msgs/msg/pose_array.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 <rclcpp/timer.hpp>
 #include <string>
 #include <tf2_msgs/msg/detail/tf_message__struct.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
@@ -16,6 +17,7 @@
 #include <gz/sim/Entity.hh>
 #include <gz/sim/EntityComponentManager.hh>
 #include <gz/sim/components/Model.hh>
+#include <thread>
 
 namespace gz_enviroment {
 using CallbackReturn =
@@ -37,7 +39,6 @@ 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;
@@ -46,6 +47,9 @@ private:
   std::shared_ptr<rbs_utils::AssemblyConfigLoader> m_config_loader;
   tf2_msgs::msg::TFMessage m_transforms;
   tf2_msgs::msg::TFMessage m_target_places;
+  rclcpp::TimerBase::SharedPtr m_timer;
+
+  std::vector<geometry_msgs::msg::TransformStamped> m_all_transforms{};
 
   std::string getGzWorldName();
   std::string createGzModelString(const std::vector<double> &pose,
@@ -53,5 +57,14 @@ private:
                                   const double &mass,
                                   const std::string &mesh_filepath,
                                   const std::string &name);
+
+  void broadcast_timer_callback() {
+    if (!m_transforms.transforms.empty()) {
+      for (auto &transform : m_transforms.transforms) {
+        m_tf2_broadcaster->sendTransform(std::move(transform));
+      }
+      // m_transforms.transforms.clear();
+    }
+  }
 };
 } // namespace gz_enviroment

env_manager/gz_enviroment/src/gz_enviroment.cpp

@@ -2,8 +2,13 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "ros_gz_bridge/convert.hpp"
+#include <algorithm>
+#include <chrono>
+#include <functional>
+#include <iterator>
 #include <memory>
 #include <rclcpp/logging.hpp>
+#include <rclcpp/utilities.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
 
@@ -30,11 +35,15 @@ CallbackReturn GzEnviroment::on_init() {
                          "Model Name: " << modelName->Data());
     }
   }
+
+  // getNode()->declare_parameter("use_sim_time", true);
+
   return CallbackReturn::SUCCESS;
 }
 
 CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
   // Configuration of parameters
+  using namespace std::chrono_literals;
   RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
   m_gz_node = std::make_shared<gz::transport::Node>();
   m_world_name = getGzWorldName();
@@ -43,15 +52,9 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
   m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
       "asp-example", getNode());
   m_follow_frames = m_config_loader->getSceneModelNames();
-  // m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
+  m_transforms = m_config_loader->getTfDataAllPossible();
-  m_transforms = m_config_loader->getTfData("bishop");
+  m_timer = getNode()->create_wall_timer(
-  // TODO add workspace viewer in Rviz
+      100ms, std::bind(&GzEnviroment::broadcast_timer_callback, this));
-  // m_config_loader->printWorkspace();
-
-  // if (!doGzSpawn())
-  // {
-  //   return CallbackReturn::ERROR;
-  // }
 
   return CallbackReturn::SUCCESS;
 }
@@ -60,11 +63,10 @@ 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_target =
+
-      std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
+  m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
 
   return CallbackReturn::SUCCESS;
 }
@@ -86,7 +88,6 @@ 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");
-  // What we should use here?
   return CallbackReturn::SUCCESS;
 }
 
@@ -123,9 +124,7 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
     all_transforms.push_back(place);
   }
 
-  for (auto &transform : all_transforms) {
+  // m_all_transforms.swap(all_transforms);
-    m_tf2_broadcaster->sendTransform(transform);
-  }
 }
 
 std::string GzEnviroment::getGzWorldName() {
@@ -153,55 +152,6 @@ std::string GzEnviroment::getGzWorldName() {
   return worlds_msg.data(0);
 }
 
-// bool GzEnviroment::doGzSpawn() {
-//   gz::msgs::EntityFactory req;
-//   gz::msgs::Boolean rep;
-//   bool result;
-//   unsigned int timeout = 5000;
-//   bool executed;
-//
-//   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::vector<double> &inertia,
-//     const double &mass, const std::string &mesh_filepath,
-//     const std::string &name) {
-//   std::string model_template =
-//       std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
-//       "<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
-//       std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
-//       std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
-//       std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
-//       "</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"

rbs_bringup/config/roboclone.yaml

@@ -0,0 +1,23 @@
+scene_config:
+  - name: arm1
+    type: rbs_arm
+    pose:
+      position:
+        x: -0.45
+        y: -2.0
+        z: 1.6
+      orientation:
+        x: 3.14159
+        y: 0.0
+        z: 0.0
+  - name: arm2
+    type: rbs_arm
+    pose:
+      position:
+        x: 0.45
+        y: -2.0
+        z: 1.6
+      orientation:
+        x: 3.14159
+        y: 0.0
+        z: 0.0

rbs_bringup/launch/multi_robot.launch.py

@@ -73,7 +73,7 @@ def launch_setup(context, *args, **kwargs):
                          convert_types=False,
                          ).merge_yamls()
     for robot in robots["scene_config"]:
-        namespace = "/" + robot["name"]
+        namespace: str = "/" + robot["name"]
         ld.append(IncludeLaunchDescription(
             PythonLaunchDescriptionSource([
                 PathJoinSubstitution([
@@ -106,9 +106,13 @@ def launch_setup(context, *args, **kwargs):
                 "hardware": hardware,
                 "launch_controllers": launch_controllers,
                 "gazebo_gui": gazebo_gui,
+                "namespace": namespace,
                 "x": str(robot["pose"]["position"]["x"]),
                 "y": str(robot["pose"]["position"]["y"]),
-                "z": str(robot["pose"]["position"]["z"])
+                "z": str(robot["pose"]["position"]["z"]),
+                "roll": str(robot["pose"]["orientation"]["x"]),
+                "pitch": str(robot["pose"]["orientation"]["y"]),
+                "yaw": str(robot["pose"]["orientation"]["z"]),
             }.items()
         ))
 
@@ -117,9 +121,12 @@ def launch_setup(context, *args, **kwargs):
             executable='create',
             arguments=[
                 '-name', robot_name,
-                '-x', "0",#str(robot["spawn_point"][0]),
+                # '-x', str(robot["pose"]["position"]["x"]),
-                '-y', "0",#str(robot["spawn_point"][1]),
+                # '-y', str(robot["pose"]["position"]["y"]),
-                '-z', "0",#str(robot["spawn_point"][2]),
+                # '-z', str(robot["pose"]["position"]["z"]),
+                # '-R', str(robot["pose"]["orientation"]["x"]),
+                # '-P', str(robot["pose"]["orientation"]["y"]),
+                # '-Y', str(robot["pose"]["orientation"]["z"]),
                 '-topic', namespace + '/robot_description'],
             output='screen',
             condition=IfCondition(sim_gazebo))

rbs_bringup/launch/rbs_robot.launch.py

@@ -39,14 +39,18 @@ def launch_setup(context, *args, **kwargs):
     x_pos = LaunchConfiguration("x")
     y_pos = LaunchConfiguration("y")
     z_pos = LaunchConfiguration("z")
+    roll = LaunchConfiguration("roll")
+    pitch = LaunchConfiguration("pitch")
+    yaw = LaunchConfiguration("yaw")
+    namespace = LaunchConfiguration("namespace")
     robot_name = robot_name.perform(context)
-    namespace = "/" + robot_name
+    namespace = namespace.perform(context)
     robot_type = robot_type.perform(context)
     description_package = description_package.perform(context)
     description_file = description_file.perform(context)
     controllers_file = controllers_file.perform(context)
 
-    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
+    # remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
     
     xacro_file = os.path.join(get_package_share_directory(description_package), "urdf", description_file)
     robot_description_doc = xacro.process_file(
@@ -89,8 +93,27 @@ def launch_setup(context, *args, **kwargs):
         executable="robot_state_publisher",
         namespace=namespace,
         output="both",
-        remappings=remappings,
+        # remappings=remappings,
-        parameters=[{"use_sim_time": True}, robot_description],
+        parameters=[{"use_sim_time": use_sim_time}, robot_description],
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
+    )
+
+    rviz = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        namespace=namespace,
+        arguments=["-d", rviz_config_file],
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            {'use_sim_time': use_sim_time}
+        ]
     )
 
     control = IncludeLaunchDescription(
@@ -182,7 +205,8 @@ def launch_setup(context, *args, **kwargs):
         moveit,
         skills,
         task_planner,
-        perception
+        perception,
+        # rviz
     ]
     return nodes_to_start
 
@@ -337,6 +361,11 @@ def generate_launch_description():
                               default_value="false", 
                               description="Launch gazebo with gui?")
     )
+    declared_arguments.append(
+        DeclareLaunchArgument("namespace", 
+                              default_value="", 
+                              description="The ROS2 namespace of a robot")
+    )
     declared_arguments.append(
         DeclareLaunchArgument("x", 
                               default_value="0.0", 

rbs_bringup/launch/single_robot.launch.py

@@ -58,27 +58,31 @@ def launch_setup(context, *args, **kwargs):
         condition=IfCondition(launch_simulation)
     )
 
-    configured_params = RewrittenYaml(
+    # FIXME: namespaces
-        source_file=os.path.join(
+    # configured_params = RewrittenYaml(
-            get_package_share_directory(
+    #     source_file=os.path.join(
-                description_package.perform(context)), 
+    #         get_package_share_directory(
-            "config", 
+    #             description_package.perform(context)), 
-            controllers_file.perform(context)),
+    #         "config", 
-        root_key=robot_name.perform(context),
+    #         controllers_file.perform(context)),
-        param_rewrites={},
+    #     root_key=robot_name.perform(context),
-        convert_types=True,
+    #     param_rewrites={},
+    #     convert_types=True,
+    # )
+
+    initial_joint_controllers_file_path = os.path.join(
+        get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml'
     )
-    controller_paramfile = configured_params.perform(context)
+
-    namespace = "/" + robot_name.perform(context) 
+    # controller_paramfile = configured_params.perform(context)
+    # namespace = "/" + robot_name.perform(context) 
+    namespace = ""
 
     gz_spawner = Node(
         package='ros_gz_sim',
         executable='create',
         arguments=[
             '-name', robot_name.perform(context),
-            '-x', "0.5",
-            '-y', "0.5",
-            '-z', "0.0",
             '-topic', namespace + '/robot_description'],
         output='screen',
         condition=IfCondition(sim_gazebo))
@@ -97,11 +101,11 @@ def launch_setup(context, *args, **kwargs):
             "gripper_name": gripper_name,
             "controllers_file": controllers_file,
             "robot_type": robot_type,
-            "controllers_file": controller_paramfile,
+            "controllers_file": initial_joint_controllers_file_path,
             "cartesian_controllers": cartesian_controllers,
             "description_package": description_package,
             "description_file": description_file,
-            "robot_name": robot_name,
+            "robot_name": robot_type,
             "start_joint_controller": start_joint_controller,
             "initial_joint_controller": initial_joint_controller,
             "launch_simulation": launch_simulation,
@@ -115,9 +119,9 @@ def launch_setup(context, *args, **kwargs):
             "hardware": hardware,
             "launch_controllers": "true",
             "gazebo_gui": gazebo_gui,
-            "x": "0.5",
+            # "x": "0.5",
-            "y": "0.5",
+            # "y": "0.5",
-            "z": "0.5"
+            # "z": "0.5"
         }.items()
     )
     

rbs_bt_executor/bt_trees/test_tree.xml

@@ -14,19 +14,19 @@
         <Sequence>
             <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
                 pose_vec_out="{goal_pose}"
-                robot_name="{robot_name}" server_name="/move_topose" server_timeout="5000" />
+                robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
             <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
                 pose_vec_out="{goal_pose}"
-                robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
+                robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
             <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
                 pose_vec_out="{goal_pose}"
-                robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
+                robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
             <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
                 pose_vec_out="{goal_pose}"
-                robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
+                robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
             <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
                 pose_vec_out="{goal_pose}"
-                robot_name="{robot_name}" server_name="/move_cartesian" server_timeout="5000" />
+                robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
 
         </Sequence>
     </BehaviorTree>

rbs_bt_executor/launch/rbs_executor.launch.py

@@ -27,11 +27,16 @@ def generate_launch_description():
         Node(
             package='behavior_tree',
             executable='bt_engine',
-            # prefix=['gdbserver localhost:3000'],
+            # prefix=['gdbserver localhost:1234'],
             parameters=[
                 btfile_param,
-                bt_skills_param
+                bt_skills_param,
-            ]
+                {'use_sim_time': True}
+            ],
+            # arguments=[
+            #     "--ros-args",
+            #     "--log-level", "debug",
+            # ],
         )
     ]
 

rbs_bt_executor/src/EnvManager.cpp

@@ -35,7 +35,7 @@ public:
     if (response->ok) {
       // TODO We need better perfomance for call it in another place for all BT nodes
       // - Make it via shared_ptr forward throught blackboard
-      auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2", _node);
+      auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example", _node);
       auto p = t->getWorkspaceInspectorTrajectory();
       setOutput("workspace", p);
       return BT::NodeStatus::SUCCESS;

rbs_bt_executor/src/MoveToPose.cpp

@@ -29,8 +29,8 @@ class MoveToPose : public BtAction<MoveToPoseAction> {
 
     goal.robot_name = robot_name_;
     goal.target_pose = pose_des;
-    goal.end_effector_acceleration = 0.5;
+    goal.end_effector_acceleration = 1.0;
-    goal.end_effector_velocity = 0.5;
+    goal.end_effector_velocity = 1.0;
 
     RCLCPP_INFO(_node->get_logger(), "Goal populated");
 

rbs_simulation/launch/simulation_gazebo.launch.py

@@ -69,6 +69,7 @@ def generate_launch_description():
     env_manager = Node(
         package="env_manager",
         executable="run_env_manager",
+        parameters=[{'use_sim_time': True}],
         condition=IfCondition(launch_env_manager)
     )
     

rbs_simulation/worlds/asm2.sdf

@@ -92,16 +92,15 @@
         </visual>
       </link>
     </model>
-    <!-- Manipulating objects -->
+    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>
+			<!-- <include> -->
-				<name>bishop</name>
+			<!-- 	<name>bishop</name> -->
-				<uri>model://bishop</uri>
+			<!-- 	<uri>model://bishop</uri> -->
-        <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose>
+   <!--      <pose>0.35 0.0 0.0 0.0 0.0 0.0</pose> -->
-			</include>
+			<!-- </include> -->
   </world>
-</sdf>

rbs_skill_servers/CMakeLists.txt

@@ -152,6 +152,7 @@ ament_export_include_directories(include)
 ament_python_install_package(${PROJECT_NAME})
 install(PROGRAMS
   scripts/test_cartesian_controller.py
+  scripts/move_to_pose.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

rbs_skill_servers/launch/skills.launch.py

@@ -38,6 +38,12 @@ def launch_setup(context, *args, **kwargs):
         ]
     )
 
+    move_to_pose = Node(
+        package="rbs_skill_servers",
+        executable="move_to_pose.py",
+        namespace=namespace
+    )
+
     gripper_control_node = Node(
         package="rbs_skill_servers",
         executable="gripper_control_action_server",
@@ -89,6 +95,7 @@ def launch_setup(context, *args, **kwargs):
         gripper_control_node,
         move_cartesian_path_action_server,
         move_joint_state_action_server,
+        move_to_pose,
         # grasp_pose_loader
     ]
     return nodes_to_start

rbs_skill_servers/scripts/test_cartesian_controller.py

@@ -1,3 +1,4 @@
+#!/usr/bin/python
 import rclpy
 from rclpy.node import Node
 import argparse

rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp

@@ -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);
 };

rbs_utils/rbs_utils/src/rbs_utils.cpp

@@ -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);
+    void *ros_msg = reinterpret_cast<void *>(&m_assembly_config);
-    dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info, asm_config_string, ros_msg);
+    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);
+        geometry_msgs::msg::TransformStamped relative_transform_stamped;
-        tp.transforms.emplace_back().child_frame_id = relative_part.relative_at;
+        relative_transform_stamped.transform =
-        tp.transforms.emplace_back().header.stamp = m_clock->now();
+            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 (!m_assembly_config.grasp_pose.empty()) {
-    if (grasp_pose.name == model_name) {
+    for (const auto &grasp_pose : m_assembly_config.grasp_pose) {
-      tp.transforms.emplace_back().transform = createTransform(grasp_pose.pose);
+      if (grasp_pose.name == model_name) {
-        tp.transforms.emplace_back().child_frame_id = grasp_pose.name;
+        geometry_msgs::msg::TransformStamped grasp_transform_stamped;
-        tp.transforms.emplace_back().header.stamp = m_clock->now();
+        grasp_transform_stamped.transform = createTransform(grasp_pose.pose);
-      found_grasp_pose = true;
+        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() {
+geometry_msgs::msg::PoseArray AssemblyConfigLoader::getWorkspace() {
-  tf2_msgs::msg::TFMessage tf_msg;
+  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 (const auto &point : m_assembly_config.workspace) {
-  for (std::size_t i = 0; i < m_assembly_config.workspace.size(); ++i) {
+    geometry_msgs::msg::Pose pose;
-    tf_msg.transforms[i].transform.translation.x =
+    pose.position.x = point.x;
-        m_assembly_config.workspace[i].x;
+    pose.position.y = point.y;
-    tf_msg.transforms[i].transform.translation.y =
+    pose.position.z = point.z;
-        m_assembly_config.workspace[i].y;
+    pose.orientation.x = default_rotation_value;
-    tf_msg.transforms[i].transform.translation.z =
+    pose.orientation.y = default_rotation_value;
-        m_assembly_config.workspace[i].z;
+    pose.orientation.z = default_rotation_value;
-    tf_msg.transforms[i].child_frame_id = child_frame_id;
+    pose.orientation.w = default_rotation_w;
-    tf_msg.transforms[i].transform.rotation.x = default_rotation_value;
+    pose_array.poses.push_back(pose);
-    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();
   }
-  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) {
+    const geometry_msgs::msg::Pose &pose) {
-  auto pose_eigen = tf2::transformToEigen(pose.transform);
+
+  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 tf2::toMsg(pose_eigen);
-  return pose_msg;
 }
 
 } // namespace rbs_utils

rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg

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