refactor: add collision spawner and planning scene manager node

- refactored places config and ref_frames config parsers
- updated dependent files

rbs_mill_assist/include/rbs_mill_assist/places_config.hpp

@@ -1,10 +1,12 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include <filesystem>
+#include <memory>
 #include <rclcpp/logger.hpp>
 #include <rclcpp/node.hpp>
+#include <rclcpp/node_interfaces/node_base_interface.hpp>
+#include <rclcpp/node_interfaces/node_logging_interface.hpp>
 #include <string>
 #include <tf2/LinearMath/Quaternion.hpp>
-#include <utility>
 #include <vector>
 #include <yaml-cpp/node/node.h>
 #include <yaml-cpp/node/parse.h>
@@ -12,32 +14,140 @@
 
 class PlacesConfig {
 public:
-  PlacesConfig(const std::string &places_filepath) {
+  PlacesConfig(const std::string &places_filepath, const rclcpp::Logger& logger)
-    loadConfig(places_filepath);
+      : _logger(logger) {
+
+    objects_to_spawn_ = std::make_shared<Places>();
+    if (!loadPlacesFromYaml(places_filepath)) {
+      RCLCPP_FATAL(_logger, "Failed to load objects from YAML file: %s",
+                   places_filepath.c_str());
+      rclcpp::shutdown();
+      return;
+    }
   }
 
-  inline std::vector<std::string> getMeshFilenames() {
+  PlacesConfig(const std::string &places_filepath, rclcpp::Node::SharedPtr node)
-    return _model_mesh_filenames;
+      : PlacesConfig(places_filepath, node->get_logger()) {}
-  }
+
-  inline std::vector<geometry_msgs::msg::Pose> getMeshPoses(){
+  struct PlaceSpec {
-    return _model_poses;
+    std::string name;
-  }
+    std::string type;
-  inline std::vector<std::string> getMeshFilepaths() {
+    std::string frame_id = "world";
-    return _model_mesh_filepaths;
+    bool has_dimensions;
-  }
+    std::vector<double> dimensions;
-  inline std::vector<std::string> getModelNames() {
+    geometry_msgs::msg::Pose pose;
-    return _model_names;
+    std::optional<std::string> mesh_filepath;
-  }
+    std::tuple<double, double, double> scale_xyz = {1.0, 1.0, 1.0};
-  inline std::vector<std::string> getModelParents() {
+  };
-    return _model_parents;
+
-  }
+  struct Places {
+    std::optional<std::string> model_dir;
+    std::vector<PlaceSpec> models;
+  };
+
+  inline std::shared_ptr<Places> get() const { return objects_to_spawn_; }
 
 private:
-  std::vector<std::string> _model_names;
+  const rclcpp::Logger& _logger;
-  std::vector<std::string> _model_mesh_filenames;
+
-  std::vector<geometry_msgs::msg::Pose> _model_poses;
+  bool loadPlacesFromYaml(const std::string &filepath) {
-  std::vector<std::string> _model_mesh_filepaths;
+    auto places_config_node = YAML::LoadFile(filepath);
-  std::vector<std::string> _model_parents;
+
+    if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
+      objects_to_spawn_->model_dir = node.as<std::string>();
+    }
+
+    if (!places_config_node["models"] ||
+        !places_config_node["models"].IsSequence()) {
+      RCLCPP_FATAL(_logger, "No valid 'models' section found in config_file");
+      return false;
+    }
+    for (const auto &model : places_config_node["models"]) {
+      PlaceSpec object;
+      if (model["ps_ignore"] && model["ps_ignore"].as<bool>()) {
+        continue;
+      }
+      object.name = model["name"].as<std::string>();
+      object.type = model["type"].as<std::string>();
+      const auto &type = object.type;
+      if (type == "box" || type == "sphere" || type == "cylinder") {
+        if (!model["dimensions"]) {
+          RCLCPP_ERROR(_logger, "Model '%s' is missing 'dimensions'",
+                       object.name.c_str());
+          return false;
+        }
+        auto d = model["dimensions"].as<std::vector<double>>();
+
+        if (type == "box" && d.size() == 3) {
+          object.dimensions = d;
+        } else if (type == "sphere" && d.size() == 1) {
+          object.dimensions = d;
+        } else if (type == "cylinder" && d.size() == 2) {
+          object.dimensions = d;
+        } else {
+          RCLCPP_ERROR(_logger, "Invalid dimensions count for type '%s'",
+                       type.c_str());
+          return false;
+        }
+
+      } else if (type == "mesh") {
+        if (!model["mesh_filename"]) {
+          RCLCPP_ERROR(_logger, "mesh object missing mesh_filename");
+          return false;
+        }
+        std::string mesh_filename = model["mesh_filename"].as<std::string>();
+        if (objects_to_spawn_->model_dir.has_value()) {
+          object.mesh_filepath =
+              std::filesystem::path(objects_to_spawn_->model_dir->c_str()) /
+              object.name / "meshes" / mesh_filename;
+          RCLCPP_WARN(_logger, "mesh filepath is: %s",
+                      object.mesh_filepath->c_str());
+        } else {
+          RCLCPP_ERROR(_logger, "mesh_filename in empty in config_file");
+          return false;
+        }
+
+      } else {
+        RCLCPP_ERROR(_logger, "Unrecognized object type: '%s'", type.c_str());
+        return false;
+      }
+      if (model["frame_id"]) {
+        object.frame_id = model["frame_id"].as<std::string>();
+      }
+      if (model["pose"]) {
+        auto pose = model["pose"];
+
+        if (pose["position"]) {
+          auto position = model["pose"]["position"];
+          object.pose.position.x = position["x"].as<double>();
+          object.pose.position.y = position["y"].as<double>();
+          object.pose.position.z = position["z"].as<double>();
+        }
+        if (pose["orientation"]) {
+          auto orientation = model["pose"]["orientation"];
+          if (!isQuat(orientation)) {
+            auto quat = rpyToQuat(orientation["r"].as<double>(),
+                                  orientation["p"].as<double>(),
+                                  orientation["y"].as<double>());
+
+            object.pose.orientation.x = quat.x;
+            object.pose.orientation.y = quat.y;
+            object.pose.orientation.z = quat.z;
+            object.pose.orientation.w = quat.w;
+          } else {
+            object.pose.orientation.x = orientation["x"].as<double>();
+            object.pose.orientation.y = orientation["y"].as<double>();
+            object.pose.orientation.z = orientation["z"].as<double>();
+            object.pose.orientation.w = orientation["w"].as<double>();
+          }
+        }
+      }
+
+      objects_to_spawn_->models.push_back(object);
+    }
+
+    return true;
+  }
 
   bool isQuat(const YAML::Node &node) {
     if (node["x"] && node["y"] && node["z"] && node["w"]) {
@@ -49,82 +159,18 @@ private:
         "Provided YAML Node is not quat or euler angles");
   }
 
-  geometry_msgs::msg::Quaternion toQuat(const YAML::Node &node) {
+  geometry_msgs::msg::Quaternion rpyToQuat(double roll, double pitch,
-    geometry_msgs::msg::Quaternion quat;
+                                           double yaw) {
-    if (isQuat(node)) {
+    tf2::Quaternion q;
-      quat.x = node["x"].as<double>();
+    q.setRPY(roll, pitch, yaw);
-      quat.y = node["y"].as<double>();
+    q.normalize();
-      quat.z = node["z"].as<double>();
+    geometry_msgs::msg::Quaternion q_msg;
-      quat.w = node["w"].as<double>();
+    q_msg.x = q.x();
-    } else {
+    q_msg.y = q.y();
-      tf2::Quaternion tf2_quat;
+    q_msg.z = q.z();
-      tf2_quat.setRPY(node["r"].as<double>(), node["p"].as<double>(),
+    q_msg.w = q.w();
-                      node["y"].as<double>());
+    return q_msg;
-      quat.x = tf2_quat.x();
-      quat.y = tf2_quat.y();
-      quat.z = tf2_quat.z();
-      quat.w = tf2_quat.w();
-    }
-    return quat;
   }
-  void loadConfig(const std::string &places_config_filepath) {
-    YAML::Node places_config_node = YAML::LoadFile(places_config_filepath);
 
-    std::string model_dir;
+  std::shared_ptr<Places> objects_to_spawn_;
-    if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
-      model_dir = node.as<std::string>();
-    }
-
-    if (!places_config_node["models"] ||
-        !places_config_node["models"].IsSequence()) {
-      throw std::runtime_error(
-          "No valid 'models' section found in places.yaml");
-    }
-
-    for (const auto &model : places_config_node["models"]) {
-      if (!model["name"] || !model["mesh_filename"] || !model["pose"]) {
-        throw std::runtime_error(
-            "Missing required model parameters in places.yaml");
-      }
-
-      std::string model_name = model["name"].as<std::string>();
-      std::string model_parent = "world";
-      if (model["relative_to"].IsDefined()) {
-        model_parent = model["relative_to"].as<std::string>();
-      }
-      std::string model_mesh_filename =
-          model["mesh_filename"].as<std::string>();
-
-      _model_names.push_back(std::move(model_name));
-      _model_mesh_filenames.push_back(std::move(model_mesh_filename));
-      _model_parents.push_back(std::move(model_parent));
-
-      if (!model_dir.empty()) {
-        std::filesystem::path mesh_path = std::filesystem::path(model_dir) /
-                                          _model_names.back() / "meshes" /
-                                          _model_mesh_filenames.back();
-        _model_mesh_filepaths.push_back(mesh_path.string());
-      }
-
-      geometry_msgs::msg::Pose model_pose;
-
-      auto position = model["pose"]["position"];
-      if (!position) {
-        throw std::runtime_error("Define the model position in places.yaml");
-      }
-
-      model_pose.position.x = position["x"].as<double>();
-      model_pose.position.y = position["y"].as<double>();
-      model_pose.position.z = position["z"].as<double>();
-
-      if (auto orientation = model["pose"]["orientation"]; orientation) {
-        model_pose.orientation = toQuat(orientation);
-      } else {
-        RCLCPP_WARN(rclcpp::get_logger("planning_scene_loader"),
-                    "Model orientation is not set. Using default values.");
-      }
-
-      _model_poses.push_back(std::move(model_pose));
-    }
-  }
 };

rbs_mill_assist/include/rbs_mill_assist/ref_frames_config.hpp

@@ -3,6 +3,7 @@
 #include <rclcpp/logger.hpp>
 #include <rclcpp/node.hpp>
 #include <stdexcept>
+#include <string>
 #include <tf2/LinearMath/Quaternion.hpp>
 #include <yaml-cpp/yaml.h>
 
@@ -11,20 +12,20 @@ public:
   RefFramesConfig(const std::string &ref_frames_filepath) {
     loadConfig(ref_frames_filepath);
   }
-  inline std::vector<std::string> getRefFrameNames() {
+
-    return _ref_frame_names;
+  struct RefFrame {
-  }
+    std::string name;
-  inline std::vector<std::string> getRefFrameParents() {
+    std::string relative_to;
-    return _ref_frame_parents;
+    geometry_msgs::msg::Pose pose;
-  }
+  };
-  inline std::vector<geometry_msgs::msg::Pose> getRefFramePoses() {
+
-    return _ref_frame_poses;
+  inline std::vector<RefFrame> get() const {
+    return _ref_frames;
   }
 
+
 private:
-  std::vector<std::string> _ref_frame_names;
+  std::vector<RefFrame> _ref_frames;
-  std::vector<std::string> _ref_frame_parents;
-  std::vector<geometry_msgs::msg::Pose> _ref_frame_poses;
 
   geometry_msgs::msg::Quaternion toQuat(const YAML::Node &node) {
     geometry_msgs::msg::Quaternion quat;
@@ -47,12 +48,13 @@ private:
 
   void parseRefFrames(const YAML::Node &node) {
     for (const auto &frame : node) {
+      RefFrame ref_frame;
       if (!frame["name"] || !frame["relative_to"] || !frame["pose"]) {
         throw std::runtime_error(
             "Missing required ref_frame parameters in YAML");
       }
-      _ref_frame_names.push_back(frame["name"].as<std::string>());
+      ref_frame.name = frame["name"].as<std::string>();
-      _ref_frame_parents.push_back(frame["relative_to"].as<std::string>());
+      ref_frame.relative_to = frame["relative_to"].as<std::string>();
 
       geometry_msgs::msg::Pose frame_pose;
       frame_pose.position.x = frame["pose"]["position"]["x"].as<double>();
@@ -62,7 +64,8 @@ private:
       if (auto orientation = frame["pose"]["orientation"]; orientation) {
         frame_pose.orientation = toQuat(orientation);
       }
-      _ref_frame_poses.push_back(frame_pose);
+      ref_frame.pose = frame_pose;
+      _ref_frames.push_back(ref_frame);
     }
   }
 

rbs_mill_assist/launch/moveit.launch.py

@@ -132,10 +132,26 @@ def launch_setup(context, *args, **kwargs):
 
 
     places_config_filepath = os.path.join(get_package_share_directory("rbs_mill_assist"), "world/config", "places.yaml")
-    planning_scene_publisher = Node(
+
+    # planning_scene_publisher = Node(
+    #     package="rbs_mill_assist",
+    #     executable="planning_scene_publisher",
+    #     parameters=[moveit_config.to_dict(), {"use_sim_time": True}, {"places_config": places_config_filepath}],
+    # )
+
+    collision_spawner = Node(
         package="rbs_mill_assist",
-        executable="planning_scene_publisher",
+        # prefix=['gdbserver localhost:1234'],
-        parameters=[moveit_config.to_dict(), {"use_sim_time": True}, {"places_config": places_config_filepath}],
+        executable="collision_spawner",
+        parameters=[{"config_file": places_config_filepath}]
+    )
+
+
+    ps_manager = Node(
+        package="rbs_mill_assist",
+        executable="ps_manager",
+        output="screen",
+        parameters=[{"frame_id": "world"}]
     )
 
     ref_frames_config_filepath = os.path.join(
@@ -155,16 +171,6 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
-
-    handler = RegisterEventHandler(
-        OnProcessStart(
-            target_action=places_frame_publisher,
-            on_start=[planning_scene_publisher],
-
-        )
-    )
-
-
     rviz_node = Node(
         package="rviz2",
         executable="rviz2",
@@ -182,18 +188,14 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
-    delay_planning_scene = TimerAction(
-        actions=[planning_scene_publisher],
-        period=5.0
-    )
-
     nodes_to_start = [
         move_group_node,
         rviz_node,
         places_frame_publisher,
-        planning_scene_publisher,
         # delay_planning_scene
         # handler
+        collision_spawner,
+        ps_manager
     ]
 
     return nodes_to_start

rbs_mill_assist/src/CMakeLists.txt

@@ -58,20 +58,20 @@ install(
 
 # planning_scene_publisher
 
-find_package(moveit_msgs REQUIRED)
+# find_package(moveit_msgs REQUIRED)
-find_package(moveit_core REQUIRED)
+# find_package(moveit_core REQUIRED)
-find_package(geometry_msgs REQUIRED)
+# find_package(geometry_msgs REQUIRED)
-find_package(shape_msgs REQUIRED)
+# find_package(shape_msgs REQUIRED)
-find_package(yaml-cpp REQUIRED)
+# find_package(yaml-cpp REQUIRED)
-
+#
-add_executable(planning_scene_publisher planning_scene_publisher.cpp)
+# add_executable(planning_scene_publisher planning_scene_publisher.cpp)
-ament_target_dependencies(planning_scene_publisher moveit_msgs moveit_core geometry_msgs)
+# ament_target_dependencies(planning_scene_publisher moveit_msgs moveit_core geometry_msgs)
-target_link_libraries(planning_scene_publisher yaml-cpp)
+# target_link_libraries(planning_scene_publisher yaml-cpp)
-
+#
-install(
+# install(
-  TARGETS planning_scene_publisher
+#   TARGETS planning_scene_publisher
-  DESTINATION lib/${PROJECT_NAME}
+#   DESTINATION lib/${PROJECT_NAME}
-)
+# )
 
 # publish_ee_pose
 

rbs_mill_assist/src/places_frame_publisher.cpp

@@ -22,7 +22,7 @@ public:
         this->get_parameter("places_config_filepath").as_string();
     auto ref_frames_config_filepath =
         this->get_parameter("ref_frames_config_filepath").as_string();
-    auto places_config = std::make_shared<PlacesConfig>(places_config_filepath);
+    auto places_config = std::make_shared<PlacesConfig>(places_config_filepath, this->get_logger());
     auto ref_frames =
         std::make_shared<RefFramesConfig>(ref_frames_config_filepath);
 
@@ -32,15 +32,16 @@ public:
 
 private:
   void publish_ref_frames(std::shared_ptr<RefFramesConfig> &ref_frames_config) {
-    for (size_t i = 0; i < ref_frames_config->getRefFrameNames().size(); i++) {
+    auto ref_frames = ref_frames_config->get();
+    for (size_t i = 0; i < ref_frames.size(); i++) {
       geometry_msgs::msg::TransformStamped t;
 
       t.header.stamp = this->get_clock()->now();
-      t.header.frame_id = ref_frames_config->getRefFrameParents()[i];
+      t.header.frame_id = ref_frames[i].relative_to;
-      t.child_frame_id = ref_frames_config->getRefFrameNames()[i];
+      t.child_frame_id = ref_frames[i].name;
 
       geometry_msgs::msg::Pose tmp_pose;
-      tmp_pose = ref_frames_config->getRefFramePoses()[i];
+      tmp_pose = ref_frames[i].pose;
 
       t.transform.translation.x = tmp_pose.position.x;
       t.transform.translation.y = tmp_pose.position.y;
@@ -54,16 +55,17 @@ private:
     }
   }
 
-  void publish_places_frames(std::shared_ptr<PlacesConfig> &places_config) {
+  void publish_places_frames(std::shared_ptr<PlacesConfig> places_config) {
-    for (size_t i = 0; i < places_config->getModelNames().size(); i++) {
+    auto places = places_config->get();
+    for (size_t i = 0; i < places->models.size(); i++) {
       geometry_msgs::msg::TransformStamped t;
 
       t.header.stamp = this->get_clock()->now();
-      t.header.frame_id = places_config->getModelParents()[i];
+      t.header.frame_id = places->models[i].frame_id;
-      t.child_frame_id = places_config->getModelNames()[i];
+      t.child_frame_id = places->models[i].name;
 
       geometry_msgs::msg::Pose tmp_pose;
-      tmp_pose = places_config->getMeshPoses()[i];
+      tmp_pose = places->models[i].pose;
 
       t.transform.translation.x = tmp_pose.position.x;
       t.transform.translation.y = tmp_pose.position.y;

rbs_mill_assist/utils/CMakeLists.txt

@@ -0,0 +1,53 @@
+
+find_package(rclcpp REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(shape_msgs REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(geometric_shapes REQUIRED)
+find_package(rbs_mill_interfaces REQUIRED)
+
+include_directories(include)
+
+add_executable(ps_manager ./src/ps_manager.cpp)
+
+ament_target_dependencies(ps_manager
+  rclcpp
+  moveit_ros_planning_interface
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  geometry_msgs
+  shape_msgs
+  moveit_msgs
+  geometric_shapes
+  rbs_mill_interfaces
+)
+
+
+add_executable(collision_spawner ./src/collision_spawner.cpp)
+ament_target_dependencies(collision_spawner
+  rclcpp
+  moveit_ros_planning_interface
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  geometry_msgs
+  shape_msgs
+  moveit_msgs
+  yaml-cpp
+  rbs_mill_interfaces
+)
+
+target_link_libraries(collision_spawner yaml-cpp)
+
+install(TARGETS
+  ps_manager
+  collision_spawner
+  DESTINATION lib/${PROJECT_NAME}
+)
+

rbs_mill_assist/utils/include/ps_manager.hpp

@@ -1,27 +0,0 @@
-
-#include <rclcpp/node_options.hpp>
-#include <rclcpp/rclcpp.hpp>
-#include <rclcpp_action/rclcpp_action.hpp>
-#include <geometry_msgs/msg/pose.hpp>
-#include <moveit_msgs/msg/collision_object.hpp>
-#include <shape_msgs/msg/solid_primitive.hpp>
-#include <geometry_msgs/msg/quaternion.hpp>
-// #include "rbs_mill_interfaces/msg/
-
-
-namespace rbs_mill_utils {
-
-class ObjectManager : public rclcpp::Node {
-
-  public:
-
-  ObjectManager(const rclcpp::NodeOptions options);
-
-  private:
-
-  void attachDetachCallback();
-
-};
-
-
-}

rbs_mill_assist/utils/include/rbs_mill_utils/collision_spawner.hpp

@@ -0,0 +1,58 @@
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "rbs_mill_interfaces/action/add_collision_object.hpp"
+#include "rbs_mill_interfaces/action/check_object_exists.hpp"
+#include "rbs_mill_interfaces/action/remove_collision_object.hpp"
+#include "rbs_mill_assist/places_config.hpp"
+#include <geometry_msgs/msg/pose.hpp>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/client.hpp>
+#include <tuple>
+#include <yaml-cpp/node/node.h>
+
+class CollisionSpawner : public rclcpp::Node {
+
+public:
+  struct ObjectSpec {
+    std::string name;
+    std::string type;
+    std::string frame_id = "world";
+    bool has_dimensions;
+    std::vector<double> dimensions;
+    geometry_msgs::msg::Pose pose;
+    std::optional<std::string> mesh_filepath;
+    std::tuple<double, double, double> scale_xyz = {1.0, 1.0, 1.0};
+  };
+
+  struct Objects {
+    std::optional<std::string> model_dir;
+    std::vector<ObjectSpec> models;
+  };
+
+  using AddCollisionObject = rbs_mill_interfaces::action::AddCollisionObject;
+  using RemoveCollisionObject =
+      rbs_mill_interfaces::action::RemoveCollisionObject;
+  using CheckObjectExists = rbs_mill_interfaces::action::CheckObjectExists;
+
+  CollisionSpawner();
+
+private:
+  void removeExistingObjects();
+
+  void sendRemoveGoal(const std::string &object_id);
+  void spawnObjects();
+
+  void sendAddGoal(const PlacesConfig::PlaceSpec &obj_spec);
+
+  bool isObjectExist(const std::string &object_id);
+
+  std::shared_ptr<PlacesConfig> _places_config;
+
+  rclcpp_action::Client<AddCollisionObject>::SharedPtr
+      add_object_action_client_;
+  rclcpp_action::Client<RemoveCollisionObject>::SharedPtr
+      remove_object_action_client_;
+  rclcpp_action::Client<CheckObjectExists>::SharedPtr
+      check_object_exists_action_client_;
+  std::string config_file_;
+};

rbs_mill_assist/utils/include/rbs_mill_utils/ps_manager.hpp

@@ -0,0 +1,142 @@
+
+#include "moveit_msgs/msg/collision_object.hpp"
+#include "rbs_mill_interfaces/action/add_collision_object.hpp"
+#include "rbs_mill_interfaces/action/attach_detach_object.hpp"
+#include "rbs_mill_interfaces/action/check_object_exists.hpp"
+#include "rbs_mill_interfaces/action/get_object_pose.hpp"
+#include "rbs_mill_interfaces/action/remove_collision_object.hpp"
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+#include <moveit_msgs/msg/attached_collision_object.hpp>
+#include <moveit_msgs/msg/collision_object.hpp>
+#include <moveit_msgs/srv/get_planning_scene.hpp>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/service.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp_action/server.hpp>
+#include <shape_msgs/msg/solid_primitive.hpp>
+
+namespace rbs_mill_utils {
+
+class PsManager : public rclcpp::Node {
+
+public:
+  using AttachDetachObject = rbs_mill_interfaces::action::AttachDetachObject;
+
+  using AddCollisionObject = rbs_mill_interfaces::action::AddCollisionObject;
+  using RemoveCollisionObject =
+      rbs_mill_interfaces::action::RemoveCollisionObject;
+  using CheckObjectExists = rbs_mill_interfaces::action::CheckObjectExists;
+  using GetObjectPose = rbs_mill_interfaces::action::GetObjectPose;
+
+  using AddGoalHandle = rclcpp_action::ServerGoalHandle<AddCollisionObject>;
+  using RemoveGoalHandle =
+      rclcpp_action::ServerGoalHandle<RemoveCollisionObject>;
+  using CheckGoalHandle = rclcpp_action::ServerGoalHandle<CheckObjectExists>;
+  using AttachDetachGoalHandle =
+      rclcpp_action::ServerGoalHandle<AttachDetachObject>;
+  using GetObjectPoseGoalHandle =
+      rclcpp_action::ServerGoalHandle<GetObjectPose>;
+
+  PsManager();
+
+private:
+  rclcpp_action::GoalResponse
+  handleAddGoal(const rclcpp_action::GoalUUID &uuid,
+                std::shared_ptr<const AddCollisionObject::Goal> goal);
+
+  rclcpp_action::CancelResponse
+  handleAddCancel(const std::shared_ptr<AddGoalHandle> goal_handle);
+  void handleAddExecute(const std::shared_ptr<AddGoalHandle> goal_handle);
+  rclcpp_action::GoalResponse
+  handleRemoveGoal(const rclcpp_action::GoalUUID &uuid,
+                   std::shared_ptr<const RemoveCollisionObject::Goal> goal);
+  rclcpp_action::CancelResponse
+  handleRemoveCancel(const std::shared_ptr<RemoveGoalHandle> goal_handle);
+  void handleRemoveExecute(const std::shared_ptr<RemoveGoalHandle> goal_handle);
+  rclcpp_action::GoalResponse
+  handleCheckExistGoal(const rclcpp_action::GoalUUID &uuid,
+                       std::shared_ptr<const CheckObjectExists::Goal> goal);
+  rclcpp_action::CancelResponse
+  handleCheckExistCancel(const std::shared_ptr<CheckGoalHandle> goal_handle);
+  void
+  handleCheckExistExecute(const std::shared_ptr<CheckGoalHandle> goal_handle);
+
+  rclcpp_action::GoalResponse
+  handleAttachDetachGoal(const rclcpp_action::GoalUUID &uuid,
+                         std::shared_ptr<const AttachDetachObject::Goal> goal);
+
+  rclcpp_action::CancelResponse handleAttachDetachCancel(
+      const std::shared_ptr<AttachDetachGoalHandle> goal_handle);
+
+  void handleAttachDetachExecute(
+      const std::shared_ptr<AttachDetachGoalHandle> goal_handle);
+
+  rclcpp_action::GoalResponse
+  handleGetObjectPoseGoal(const rclcpp_action::GoalUUID &uuid,
+                          std::shared_ptr<const GetObjectPose::Goal> goal);
+
+  rclcpp_action::CancelResponse handleGetObjectPoseCancel(
+      const std::shared_ptr<GetObjectPoseGoalHandle> goal_handle);
+
+  void handleGetObjectPoseExecute(
+      const std::shared_ptr<GetObjectPoseGoalHandle> goal_handle);
+
+  moveit_msgs::msg::CollisionObject createCollisionObject(
+      const std::string &id, const std::string &shape,
+      const std::vector<double> &dimensions, const std::string &frame_id,
+      const geometry_msgs::msg::Pose &pose, const std::string &mesh_file,
+      const double scale_mesh_x, const double scale_mesh_y,
+      const double scale_mesh_z) const;
+
+  bool attachedObjectExists(const std::string &id,
+                            const std::string &link_name = "");
+
+  std::optional<std::string>
+  getAttachedObjectLinkById(const std::string &object_id);
+
+  bool objectExists(const std::string &id);
+
+  std::optional<moveit_msgs::msg::CollisionObject>
+  getObjectDataById(const std::string &object_id);
+
+  void printPlanningScene();
+
+  bool
+  validateShapeAndDimensions(const std::string &shape,
+                             const std::vector<double> &dimensions,
+                             const std::optional<std::string> &mesh_file) const;
+
+  rclcpp::Publisher<moveit_msgs::msg::CollisionObject>::SharedPtr
+      collision_object_publisher_;
+
+  rclcpp::Publisher<moveit_msgs::msg::AttachedCollisionObject>::SharedPtr
+      attached_collision_object_publisher_;
+  rclcpp::Client<moveit_msgs::srv::GetPlanningScene>::SharedPtr
+      get_planning_scene_client_;
+
+  rclcpp_action::Server<AddCollisionObject>::SharedPtr
+      add_object_action_server_;
+
+  rclcpp_action::Server<RemoveCollisionObject>::SharedPtr
+      remove_object_action_server_;
+
+  rclcpp_action::Server<CheckObjectExists>::SharedPtr
+      check_object_exists_action_server_;
+
+  rclcpp_action::Server<AttachDetachObject>::SharedPtr
+      attach_detach_object_action_server_;
+
+  rclcpp_action::Server<GetObjectPose>::SharedPtr
+      get_object_pose_action_server_;
+
+  std::string frame_id_;
+
+  rclcpp::CallbackGroup::SharedPtr service_callback_group_;
+  rclcpp::CallbackGroup::SharedPtr action_callback_group_;
+};
+
+} // namespace rbs_mill_utils

rbs_mill_assist/utils/src/collision_spawner.cpp

@@ -0,0 +1,432 @@
+#include "rbs_mill_utils/collision_spawner.hpp"
+#include <chrono>
+#include <filesystem>
+#include <memory>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/utilities.hpp>
+#include <rclcpp_action/create_client.hpp>
+#include <tf2/LinearMath/Quaternion.hpp>
+#include <yaml-cpp/yaml.h>
+
+CollisionSpawner::CollisionSpawner() : Node("collision_spawner") {
+  this->declare_parameter("config_file", "");
+  config_file_ = this->get_parameter("config_file").as_string();
+
+  if (config_file_.empty()) {
+    RCLCPP_FATAL(this->get_logger(), "Parameter 'config_file' if missing");
+    rclcpp::shutdown();
+    return;
+  }
+
+  RCLCPP_FATAL(this->get_logger(), "Starting loading yaml");
+
+  try {
+    _places_config =
+        std::make_shared<PlacesConfig>(config_file_, this->get_logger());
+  } catch (const std::exception &ex) {
+    RCLCPP_FATAL(this->get_logger(), "Failure because: %s", ex.what());
+  }
+
+  RCLCPP_FATAL(this->get_logger(), "Ended loading yaml");
+
+  if (_places_config->get()->models.empty()) {
+    RCLCPP_WARN(this->get_logger(), "No valid objects to spawn");
+    return;
+  }
+
+  add_object_action_client_ = rclcpp_action::create_client<AddCollisionObject>(
+      this, "add_collision_object");
+  remove_object_action_client_ =
+      rclcpp_action::create_client<RemoveCollisionObject>(
+          this, "remove_collision_object");
+  check_object_exists_action_client_ =
+      rclcpp_action::create_client<CheckObjectExists>(this,
+                                                      "check_object_exists");
+
+  if (!add_object_action_client_->wait_for_action_server(
+          std::chrono::seconds(10))) {
+    RCLCPP_FATAL(this->get_logger(),
+                 "AddCollisionObject action server not available. shutdown");
+    rclcpp::shutdown();
+    return;
+  }
+
+  if (!remove_object_action_client_->wait_for_action_server(
+          std::chrono::seconds(10))) {
+    RCLCPP_FATAL(this->get_logger(),
+                 "RemoveCollisionObject action server not available. shutdown");
+    rclcpp::shutdown();
+    return;
+  }
+
+  if (!check_object_exists_action_client_->wait_for_action_server(
+          std::chrono::seconds(10))) {
+    RCLCPP_WARN(this->get_logger(), "CheckObjectExist is not available");
+  }
+
+  removeExistingObjects();
+  spawnObjects();
+}
+
+// bool CollisionSpawner::loadObjectsFromYaml(const std::string &filepath) {
+//   YAML::Node places_config_node = YAML::LoadFile(filepath);
+//
+//   std::string model_dir;
+//   if (auto node = places_config_node["model_dir"]; node && node.IsScalar()) {
+//     objects_to_spawn_.model_dir = node.as<std::string>();
+//   }
+//
+//   if (!places_config_node["models"] ||
+//       !places_config_node["models"].IsSequence()) {
+//     RCLCPP_FATAL(this->get_logger(),
+//                  "No valid 'models' section found in config_file");
+//     return false;
+//   }
+//   for (const auto &model : places_config_node["models"]) {
+//     ObjectSpec object;
+//     if (model["ps_ignore"] && model["ps_ignore"].as<bool>()) {
+//       continue;
+//     }
+//     object.name = model["name"].as<std::string>();
+//     object.type = model["type"].as<std::string>();
+//     const auto &type = object.type;
+//     if (type == "box" || type == "sphere" || type == "cylinder") {
+//       if (!model["dimensions"]) {
+//         RCLCPP_ERROR(this->get_logger(), "Model '%s' is missing
+//         'dimensions'",
+//                      object.name.c_str());
+//         return false;
+//       }
+//       auto d = model["dimensions"].as<std::vector<double>>();
+//
+//       if (type == "box" && d.size() == 3) {
+//         object.dimensions = d;
+//       } else if (type == "sphere" && d.size() == 1) {
+//         object.dimensions = d;
+//       } else if (type == "cylinder" && d.size() == 2) {
+//         object.dimensions = d;
+//       } else {
+//         RCLCPP_ERROR(this->get_logger(),
+//                      "Invalid dimensions count for type '%s'", type.c_str());
+//         return false;
+//       }
+//
+//     } else if (type == "mesh") {
+//       if (!model["mesh_filename"]) {
+//         RCLCPP_ERROR(this->get_logger(), "mesh object missing
+//         mesh_filename"); return false;
+//       }
+//       std::string mesh_filename = model["mesh_filename"].as<std::string>();
+//       if (objects_to_spawn_.model_dir.has_value()) {
+//         object.mesh_filepath =
+//             std::filesystem::path(objects_to_spawn_.model_dir->c_str()) /
+//             object.name / "meshes" / mesh_filename;
+//         RCLCPP_WARN(this->get_logger(), "mesh filepath is: %s",
+//                     object.mesh_filepath->c_str());
+//       } else {
+//         RCLCPP_ERROR(this->get_logger(),
+//                      "mesh_filename in empty in config_file");
+//         return false;
+//       }
+//
+//     } else {
+//       RCLCPP_ERROR(this->get_logger(), "Unrecognized object type: '%s'",
+//                    type.c_str());
+//       return false;
+//     }
+//     if (model["frame_id"]) {
+//       object.frame_id = model["frame_id"].as<std::string>();
+//     }
+//     if (model["pose"]) {
+//       auto pose = model["pose"];
+//
+//       if (pose["position"]) {
+//         auto position = model["pose"]["position"];
+//         object.pose.position.x = position["x"].as<double>();
+//         object.pose.position.y = position["y"].as<double>();
+//         object.pose.position.z = position["z"].as<double>();
+//       }
+//       if (pose["orientation"]) {
+//         auto orientation = model["pose"]["orientation"];
+//         if (!isQuat(orientation)) {
+//           auto quat = rpyToQuat(orientation["r"].as<double>(),
+//                                 orientation["p"].as<double>(),
+//                                 orientation["y"].as<double>());
+//
+//           object.pose.orientation.x = quat.x;
+//           object.pose.orientation.y = quat.y;
+//           object.pose.orientation.z = quat.z;
+//           object.pose.orientation.w = quat.w;
+//         } else {
+//           object.pose.orientation.x = orientation["x"].as<double>();
+//           object.pose.orientation.y = orientation["y"].as<double>();
+//           object.pose.orientation.z = orientation["z"].as<double>();
+//           object.pose.orientation.w = orientation["w"].as<double>();
+//         }
+//       }
+//     }
+//
+//     objects_to_spawn_.models.push_back(object);
+//   }
+//
+//   return true;
+// }
+
+void CollisionSpawner::removeExistingObjects() {
+  RCLCPP_INFO(this->get_logger(), "Removing existing objects...");
+  for (const auto &obj : _places_config->get()->models) {
+    auto object_id = obj.name;
+
+    // 1. Check if it exists
+    if (!isObjectExist(object_id)) {
+      RCLCPP_INFO(this->get_logger(),
+                  "Object '%s' does not exist. Skipping removal.",
+                  object_id.c_str());
+      continue;
+    }
+
+    // 2. Proceed with remove
+    sendRemoveGoal(object_id);
+  }
+}
+
+void CollisionSpawner::sendRemoveGoal(const std::string &object_id) {
+  auto goal = RemoveCollisionObject::Goal();
+  goal.id = object_id;
+
+  RCLCPP_INFO(this->get_logger(), "Sending remove goal for object '%s'.",
+              object_id.c_str());
+
+  auto send_goal_future = remove_object_action_client_->async_send_goal(goal);
+
+  // Wait for the goal to be accepted
+  if (rclcpp::spin_until_future_complete(
+          this->get_node_base_interface(), send_goal_future,
+          std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_WARN(this->get_logger(),
+                "Timeout while sending remove goal for object '%s'.",
+                object_id.c_str());
+    return;
+  }
+
+  auto goal_handle = send_goal_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(this->get_logger(),
+                 "Remove goal was rejected for object '%s'.",
+                 object_id.c_str());
+    return;
+  }
+
+  // Wait for the result
+  auto get_result_future =
+      remove_object_action_client_->async_get_result(goal_handle);
+
+  if (rclcpp::spin_until_future_complete(
+          this->get_node_base_interface(), get_result_future,
+          std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_WARN(this->get_logger(),
+                "Timeout while waiting for remove result for object '%s'.",
+                object_id.c_str());
+    return;
+  }
+
+  auto result = get_result_future.get();
+  switch (result.code) {
+  case rclcpp_action::ResultCode::SUCCEEDED:
+    RCLCPP_INFO(this->get_logger(), "Successfully removed object '%s'.",
+                object_id.c_str());
+    break;
+  case rclcpp_action::ResultCode::ABORTED:
+    RCLCPP_WARN(this->get_logger(),
+                "Remove action was aborted for object '%s'.",
+                object_id.c_str());
+    break;
+  case rclcpp_action::ResultCode::CANCELED:
+    RCLCPP_WARN(this->get_logger(),
+                "Remove action was canceled for object '%s'.",
+                object_id.c_str());
+    break;
+  default:
+    RCLCPP_WARN(this->get_logger(),
+                "Unknown result code for remove action of object '%s'.",
+                object_id.c_str());
+    break;
+  }
+}
+
+void CollisionSpawner::spawnObjects() {
+  for (const auto &obj : _places_config->get()->models) {
+    sendAddGoal(obj);
+  }
+}
+
+void CollisionSpawner::sendAddGoal(const PlacesConfig::PlaceSpec &obj_spec) {
+
+  auto goal = AddCollisionObject::Goal();
+  goal.id = obj_spec.name;
+  goal.shape = obj_spec.type;
+  goal.dimensions =
+      obj_spec.type == "mesh" ? std::vector<double>() : obj_spec.dimensions;
+  goal.pose = obj_spec.pose;
+
+  if (obj_spec.type == "mesh") {
+    if (!obj_spec.mesh_filepath.has_value()) {
+      RCLCPP_FATAL(this->get_logger(),
+                   "Mesh filepath has no value for name: %s",
+                   obj_spec.name.c_str());
+    }
+    goal.mesh_file = obj_spec.mesh_filepath.value();
+    auto [sx, sy, sz] = obj_spec.scale_xyz;
+    goal.scale_mesh_x = sx;
+    goal.scale_mesh_y = sy;
+    goal.scale_mesh_z = sz;
+  }
+
+  RCLCPP_INFO(this->get_logger(), "Sending add goal for object '%s'.",
+              obj_spec.name.c_str());
+
+  auto send_goal_future = add_object_action_client_->async_send_goal(goal);
+
+  // Wait for the goal to be accepted
+  if (rclcpp::spin_until_future_complete(
+          this->get_node_base_interface(), send_goal_future,
+          std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_WARN(this->get_logger(),
+                "Timeout while sending add goal for object '%s'.",
+                obj_spec.name.c_str());
+    return;
+  }
+
+  auto goal_handle = send_goal_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(this->get_logger(), "Add goal was rejected for object '%s'.",
+                 obj_spec.name.c_str());
+    return;
+  }
+
+  // Wait for the result
+  auto get_result_future =
+      add_object_action_client_->async_get_result(goal_handle);
+
+  if (rclcpp::spin_until_future_complete(
+          this->get_node_base_interface(), get_result_future,
+          std::chrono::seconds(5)) != rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_WARN(this->get_logger(),
+                "Timeout while waiting for add result for object '%s'.",
+                obj_spec.name.c_str());
+    return;
+  }
+
+  auto result = get_result_future.get();
+  switch (result.code) {
+  case rclcpp_action::ResultCode::SUCCEEDED:
+    RCLCPP_INFO(this->get_logger(), "Successfully added object '%s'.",
+                obj_spec.name.c_str());
+    break;
+  case rclcpp_action::ResultCode::ABORTED:
+    RCLCPP_WARN(this->get_logger(), "Add action was aborted for object '%s'.",
+                obj_spec.name.c_str());
+    break;
+  case rclcpp_action::ResultCode::CANCELED:
+    RCLCPP_WARN(this->get_logger(), "Add action was canceled for object '%s'.",
+                obj_spec.name.c_str());
+    break;
+  default:
+    RCLCPP_WARN(this->get_logger(),
+                "Unknown result code for add action of object '%s'.",
+                obj_spec.name.c_str());
+    break;
+  }
+}
+
+bool CollisionSpawner::isObjectExist(const std::string &object_id) {
+  if (!check_object_exists_action_client_->action_server_is_ready()) {
+    RCLCPP_WARN(this->get_logger(),
+                "CheckObjectExists action server not ready. Assuming object "
+                "'%s' does not exist.",
+                object_id.c_str());
+    return false;
+  }
+
+  auto goal_msg = CheckObjectExists::Goal();
+  goal_msg.object_id = object_id;
+
+  RCLCPP_INFO(this->get_logger(),
+              "Sending CheckObjectExists goal for object '%s'.",
+              object_id.c_str());
+
+  auto send_goal_future =
+      check_object_exists_action_client_->async_send_goal(goal_msg);
+
+  // Wait for the goal to be accepted
+  if (rclcpp::spin_until_future_complete(
+          this->get_node_base_interface(), send_goal_future,
+          std::chrono::seconds(3)) != rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_WARN(this->get_logger(),
+                "Timeout while sending CheckObjectExists goal for object '%s'. "
+                "Assuming it does not exist.",
+                object_id.c_str());
+    return false;
+  }
+
+  auto goal_handle = send_goal_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(this->get_logger(),
+                 "CheckObjectExists goal was rejected for object '%s'. "
+                 "Assuming it does not exist.",
+                 object_id.c_str());
+    return false;
+  }
+
+  // Wait for the result
+  auto get_result_future =
+      check_object_exists_action_client_->async_get_result(goal_handle);
+
+  if (rclcpp::spin_until_future_complete(
+          this->get_node_base_interface(), get_result_future,
+          std::chrono::seconds(3)) != rclcpp::FutureReturnCode::SUCCESS) {
+    RCLCPP_WARN(this->get_logger(),
+                "Timeout while waiting for CheckObjectExists result for object "
+                "'%s'. Assuming it does not exist.",
+                object_id.c_str());
+    return false;
+  }
+
+  auto result = get_result_future.get().result;
+  RCLCPP_INFO(this->get_logger(),
+              "CheckObjectExists result for object '%s': exists=%s",
+              object_id.c_str(), result->ok ? "true" : "false");
+  return result->ok;
+}
+
+// bool CollisionSpawner::isQuat(const YAML::Node &node) {
+//   if (node["x"] && node["y"] && node["z"] && node["w"]) {
+//     return true;
+//   } else if (node["r"] && node["p"] && node["y"]) {
+//     return false;
+//   }
+//   throw std::invalid_argument("Provided YAML Node is not quat or euler
+//   angles");
+// }
+
+// geometry_msgs::msg::Quaternion
+// CollisionSpawner::rpyToQuat(double roll, double pitch, double yaw) {
+//   tf2::Quaternion q;
+//   q.setRPY(roll, pitch, yaw);
+//   q.normalize();
+//   geometry_msgs::msg::Quaternion q_msg;
+//   q_msg.x = q.x();
+//   q_msg.y = q.y();
+//   q_msg.z = q.z();
+//   q_msg.w = q.w();
+//   return q_msg;
+// }
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<CollisionSpawner>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

rbs_mill_assist/world/config/places.yaml

@@ -1,7 +1,19 @@
 model_dir: package://rbs_mill_assist/assets/
 models:
+  - name: table
+    type: box
+    dimensions: [1.2, 0.7, 0.8]
+    gz_ignore: true
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: -0.4
+
   - name: bunker
     mesh_filename: Bunker_50mm_height_for_labels_60х40.STL
+    type: mesh
+    gz_ignore: true
     #relative_to: world
     pose:
       position:
@@ -15,6 +27,7 @@ models:
 
   - name: bunker_4_slots
     mesh_filename: Bunker_50mm_height_for_labels_60х40_4.STL
+    type: mesh
     pose:
       position:
         x: -0.150
@@ -27,6 +40,9 @@ models:
 
   - name: aruco_1
     mesh_filename: aruco_template.obj
+    type: mesh
+    ps_ignore: true
+    gz_ignore: true
     pose:
       position:
         x: -0.235
@@ -39,6 +55,9 @@ models:
 
   - name: aruco_2
     mesh_filename: aruco_template.obj
+    type: mesh
+    ps_ignore: true
+    gz_ignore: true
     pose:
       position:
         x: -0.191
@@ -51,6 +70,9 @@ models:
 
   - name: aruco_3
     mesh_filename: aruco_template.obj
+    type: mesh
+    ps_ignore: true
+    gz_ignore: true
     pose:
       position:
         x: -0.147
@@ -63,6 +85,9 @@ models:
 
   - name: aruco_4
     mesh_filename: aruco_template.obj
+    ps_ignore: true
+    gz_ignore: true
+    type: mesh
     pose:
       position:
         x: -0.103
@@ -87,14 +112,16 @@ models:
 
   - name: conductor
     mesh_filename: conductor.stl
+    type: mesh
     pose:
       position:
-        x: 0.220
+        x: 0.210
         y: -0.170
         z: 0.01
 
   - name: laser
     mesh_filename: laser.dae
+    type: mesh
     pose:
       position:
         x: 0.250

rbs_mill_assist/world/config/ref_frames.yaml

@@ -40,6 +40,43 @@ ref_frames:
         p: 0.0
         y: 0.0
 
+  - name: preparing_grasp_slot
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.2
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: preparing_place_slot
+    relative_to: bunker
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.2
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: to_graver_prom
+    relative_to: world
+    pose:
+      position:
+        x: 0.06557199290989431
+        y: 0.13898459045940778
+        z: 0.12372277569863609
+      orientation:
+        x: 0.9350606047731165
+        y: -0.3537940621486448
+        z: 0.00805682110103744
+        w: 0.02065223047131592
+
   - name: pregrasp_1
     relative_to: slot_1
     pose:

rbs_mill_assist/world/inc/from_places.xacro

@@ -4,48 +4,47 @@
   <xacro:macro name="from_places" params="models:=^">
     <xacro:if value="${models}">
       <xacro:property name="model" value="${models.pop(0)}" />
-      <xacro:model model_name="${model['name']}" static="true">
+      <xacro:if value="${model.get('gz_ignore', False) is False}">
-        <xacro:if value="${model.get('relative_to', None) is not None}" >
+        <xacro:model model_name="${model['name']}" static="true">
-          <pose relative_to="${model['relative_to']}">
+          <xacro:if value="${model.get('relative_to', None) is not None}">
-            ${model["pose"]["position"]["x"]}
+            <pose relative_to="${model['relative_to']}"> ${model["pose"]["position"]["x"]}
-            ${model["pose"]["position"]["y"]}
+              ${model["pose"]["position"]["y"]} ${model["pose"]["position"]["z"]} <xacro:if
-            ${model["pose"]["position"]["z"]}
+                value="${'orientation' in model['pose']}">
-            
+                <xacro:property name="orientation" value="${model['pose']['orientation']}" />
-            <xacro:if value="${'orientation' in model['pose']}">
+                <xacro:if
-              <xacro:property name="orientation" value="${model['pose']['orientation']}"/>
+                  value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
-              <xacro:if value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
+              ${orientation['r']}
-                ${orientation['r']}
+                  ${orientation['p']}
-                ${orientation['p']}
+                  ${orientation['y']}
-                ${orientation['y']}
+                </xacro:if>
               </xacro:if>
-            </xacro:if>
             
-            <xacro:unless value="${'orientation' in model['pose']}">
+            <xacro:unless
-              0 0 0
+                value="${'orientation' in model['pose']}">
-            </xacro:unless>
+                0 0 0
-          </pose>
+              </xacro:unless>
-        </xacro:if>
+            </pose>
-        <xacro:unless value="${model.get('relative_to', None) is not None}">
+          </xacro:if>
-          <pose>
+          <xacro:unless value="${model.get('relative_to', None) is not None}">
-            ${model["pose"]["position"]["x"]}
+            <pose> ${model["pose"]["position"]["x"]} ${model["pose"]["position"]["y"]}
-            ${model["pose"]["position"]["y"]}
+              ${model["pose"]["position"]["z"]} <xacro:if value="${'orientation' in model['pose']}">
-            ${model["pose"]["position"]["z"]}
+                <xacro:property name="orientation" value="${model['pose']['orientation']}" />
-            
+                <xacro:if
-            <xacro:if value="${'orientation' in model['pose']}">
+                  value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
-              <xacro:property name="orientation" value="${model['pose']['orientation']}"/>
+              ${orientation['r']}
-              <xacro:if value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
+                  ${orientation['p']}
-                ${orientation['r']}
+                  ${orientation['y']}
-                ${orientation['p']}
+                </xacro:if>
-                ${orientation['y']}
               </xacro:if>
-            </xacro:if>
             
-            <xacro:unless value="${'orientation' in model['pose']}">
+            <xacro:unless
-              0 0 0
+                value="${'orientation' in model['pose']}">
-            </xacro:unless>
+                0 0 0
-          </pose>
+              </xacro:unless>
-        </xacro:unless>
+            </pose>
-      </xacro:model>
+          </xacro:unless>
+        </xacro:model>
+      </xacro:if>
       <xacro:from_places />
     </xacro:if>
   </xacro:macro>

rbs_mill_interfaces/CMakeLists.txt

@@ -11,6 +11,11 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+    "action/AttachDetachObject.action"
+    "action/GetObjectPose.action"
+    "action/CheckObjectExists.action"
+    "action/AddCollisionObject.action"
+    "action/RemoveCollisionObject.action"
     "msg/GraspingPose.msg"
     "srv/GetGraspingPose.srv"
     DEPENDENCIES geometry_msgs

rbs_mill_interfaces/action/AddCollisionObject.action

@@ -0,0 +1,17 @@
+string id
+string shape
+string mesh_file
+float64[] dimensions
+
+string frame_id 'world'
+geometry_msgs/Pose pose
+
+float64 scale_mesh_x
+float64 scale_mesh_y
+float64 scale_mesh_z
+
+---
+bool ok
+string message
+---
+string status

rbs_mill_interfaces/action/AttachDetachObject.action

@@ -0,0 +1,9 @@
+string object_id
+string link_name
+bool attach
+string[] touch_links
+---
+bool ok
+string message
+---
+string status

rbs_mill_interfaces/action/CheckObjectExists.action

@@ -0,0 +1,8 @@
+string object_id
+---
+bool ok
+bool is_attached
+string link_name
+string message
+---
+

rbs_mill_interfaces/action/GetObjectPose.action

@@ -0,0 +1,10 @@
+string object_id
+string link_name
+float64[] pre_transform_xyz_rpy
+float64[] post_transform_xyz_rpy
+---
+geometry_msgs/Pose pose
+bool ok
+string message
+---
+string status

rbs_mill_interfaces/action/RemoveCollisionObject.action

@@ -0,0 +1,6 @@
+string id
+---
+bool ok
+string message
+---
+string status