add planning scene and TF publishers from configs

rbs_mill_assist/src/CMakeLists.txt

@@ -62,9 +62,11 @@ find_package(moveit_msgs REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(shape_msgs REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 add_executable(planning_scene_publisher planning_scene_publisher.cpp)
 ament_target_dependencies(planning_scene_publisher moveit_msgs moveit_core geometry_msgs)
+target_link_libraries(planning_scene_publisher yaml-cpp)
 
 install(
   TARGETS planning_scene_publisher
@@ -93,3 +95,13 @@ install(
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME})
 
+# publish places frames
+
+add_executable(places_frame_publisher places_frame_publisher.cpp)
+ament_target_dependencies(places_frame_publisher tf2_ros)
+target_link_libraries(places_frame_publisher yaml-cpp)
+
+install(
+  TARGETS places_frame_publisher
+  DESTINATION lib/${PROJECT_NAME}
+)

rbs_mill_assist/src/places_frame_publisher.cpp

@@ -0,0 +1,86 @@
+#include <ctime>
+#include <memory>
+
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "rbs_mill_assist/places_config.hpp"
+#include "rbs_mill_assist/ref_frames_config.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+
+class PlacesFramePublisher : public rclcpp::Node {
+public:
+  explicit PlacesFramePublisher() : Node("static_turtle_tf2_broadcaster") {
+    tf_static_broadcaster_ =
+        std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
+    this->declare_parameter("places_config_filepath", "");
+    this->declare_parameter("ref_frames_config_filepath", "");
+    auto places_config_filepath =
+        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 ref_frames =
+        std::make_shared<RefFramesConfig>(ref_frames_config_filepath);
+
+    this->publish_places_frames(places_config);
+    this->publish_ref_frames(ref_frames);
+  }
+
+private:
+  void publish_ref_frames(std::shared_ptr<RefFramesConfig> &ref_frames_config) {
+    for (size_t i = 0; i < ref_frames_config->getRefFrameNames().size(); i++) {
+      geometry_msgs::msg::TransformStamped t;
+
+      t.header.stamp = this->get_clock()->now();
+      t.header.frame_id = ref_frames_config->getRefFrameParents()[i];
+      t.child_frame_id = ref_frames_config->getRefFrameNames()[i];
+
+      geometry_msgs::msg::Pose tmp_pose;
+      tmp_pose = ref_frames_config->getRefFramePoses()[i];
+
+      t.transform.translation.x = tmp_pose.position.x;
+      t.transform.translation.y = tmp_pose.position.y;
+      t.transform.translation.z = tmp_pose.position.z;
+      t.transform.rotation.x = tmp_pose.orientation.x;
+      t.transform.rotation.y = tmp_pose.orientation.y;
+      t.transform.rotation.z = tmp_pose.orientation.z;
+      t.transform.rotation.w = tmp_pose.orientation.w;
+
+      tf_static_broadcaster_->sendTransform(t);
+    }
+  }
+
+  void publish_places_frames(std::shared_ptr<PlacesConfig> &places_config) {
+    for (size_t i = 0; i < places_config->getModelNames().size(); i++) {
+      geometry_msgs::msg::TransformStamped t;
+
+      t.header.stamp = this->get_clock()->now();
+      t.header.frame_id = "world";
+      t.child_frame_id = places_config->getModelNames()[i];
+
+      geometry_msgs::msg::Pose tmp_pose;
+      tmp_pose = places_config->getMeshPoses()[i];
+
+      t.transform.translation.x = tmp_pose.position.x;
+      t.transform.translation.y = tmp_pose.position.y;
+      t.transform.translation.z = tmp_pose.position.z;
+      t.transform.rotation.x = tmp_pose.orientation.x;
+      t.transform.rotation.y = tmp_pose.orientation.y;
+      t.transform.rotation.z = tmp_pose.orientation.z;
+      t.transform.rotation.w = tmp_pose.orientation.w;
+
+      tf_static_broadcaster_->sendTransform(t);
+    }
+  }
+
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
+};
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<PlacesFramePublisher>());
+  rclcpp::shutdown();
+  return 0;
+}

rbs_mill_assist/src/planning_scene_publisher.cpp

@@ -1,47 +1,63 @@
 #include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
 #include "moveit_msgs/msg/collision_object.hpp"
 #include "moveit_msgs/msg/planning_scene.hpp"
 #include "shape_msgs/msg/mesh.hpp"
 #include <chrono>
 #include <functional>
 #include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/logging.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "rbs_mill_assist/places_config.hpp"
+#include <geometric_shapes/shape_operations.h>
 #include <moveit_msgs/msg/collision_object.hpp>
 #include <moveit_msgs/msg/planning_scene.hpp>
-#include <shape_msgs/msg/solid_primitive.hpp>
 #include <shape_msgs/msg/mesh.hpp>
-#include <geometric_shapes/shape_operations.h>
+#include <shape_msgs/msg/solid_primitive.hpp>
-#include <stdexcept>
 
 class PlanningScenePublisher : public rclcpp::Node {
 public:
-  PlanningScenePublisher() : Node("planning_scene_publisher"), object_published_(false) {
+  PlanningScenePublisher()
-    planning_scene_publisher_ =
+      : Node("planning_scene_publisher"), object_published_(false) {
+    this->declare_parameter("places_config", "");
+    _planning_scene_publisher =
         this->create_publisher<moveit_msgs::msg::PlanningScene>(
             "planning_scene", 10);
-    timer_ = this->create_wall_timer(
+    _timer = this->create_wall_timer(
         std::chrono::seconds(1),
         std::bind(&PlanningScenePublisher::publish_scene, this));
+    auto places_config_filepath =
+        this->get_parameter("places_config").as_string();
+
+    try {
+      _places_config = std::make_shared<PlacesConfig>(places_config_filepath);
+    } catch (const std::exception &e) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to load config: %s", e.what());
+    }
+    // _places_config = std::make_shared<PlacesConfig>(this->shared_from_this(),
+    //                                                 places_config_filepath);
   }
 
 private:
-  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr planning_scene_publisher_;
+  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr
-  rclcpp::TimerBase::SharedPtr timer_;
+      _planning_scene_publisher;
+  rclcpp::TimerBase::SharedPtr _timer;
   bool object_published_;
+  std::shared_ptr<PlacesConfig> _places_config;
 
   void publish_scene() {
     if (object_published_) {
       return;
     }
-    
+
     moveit_msgs::msg::PlanningScene planning_scene;
     moveit_msgs::msg::CollisionObject collision_object;
-    
+
     collision_object.header.frame_id = "world";
     collision_object.id = "table";
-    
+
     shape_msgs::msg::SolidPrimitive table;
     table.type = shape_msgs::msg::SolidPrimitive::BOX;
     table.dimensions = {1.2, 0.7, 0.8};
@@ -50,50 +66,40 @@ private:
     table_pose.position.x = 0.0;
     table_pose.position.y = 0.0;
     table_pose.position.z = -0.4;
-    
+
     collision_object.primitives.push_back(table);
     collision_object.primitive_poses.push_back(table_pose);
     collision_object.operation = moveit_msgs::msg::CollisionObject::ADD;
 
     planning_scene.world.collision_objects.push_back(collision_object);
-    
+
     moveit_msgs::msg::CollisionObject mesh_object;
     mesh_object.header.frame_id = "world";
     mesh_object.id = "mesh_object";
 
-    std::vector<std::string> meshes = {
+    for (size_t i = 0; i < _places_config->getMeshFilepaths().size(); i++) {
-      "package://rbs_mill_assist/assets/laser/meshes/laser.dae",
+      auto mesh = getMeshCollisionObject(_places_config->getMeshFilepaths()[i]);
-      "package://rbs_mill_assist/assets/bunker/meshes/Bunker_50mm_height_for_labels_60х40.STL",
-      "package://rbs_mill_assist/assets/bunker_4_slots/meshes/Bunker_50mm_height_for_labels_60х40_4.STL"
-    };
-
-    std::vector<std::vector<double>> mesh_poses = {
-      {0.350, -0.170, 0.0, 0.0, 0.0, 1.0, 0.0},
-      {-0.050, 0.250, 0.0, 0.0, 0.0, 0.707, 0.707},
-      {-0.470, 0.250, 0.0, 0.0, 0.0, 0.707, 0.707},
-    };
-
-    for (size_t i=0; i < meshes.size(); i++) {
-      auto mesh = getMeshCollisionObject(meshes[i]);
-      auto mesh_pose = arrayToPose(mesh_poses[i]);
       mesh_object.meshes.push_back(mesh);
-      mesh_object.mesh_poses.push_back(mesh_pose);
     }
 
+    mesh_object.mesh_poses = _places_config->getMeshPoses();
+
     mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD;
 
     planning_scene.world.collision_objects.push_back(mesh_object);
     planning_scene.is_diff = true;
-    
+
-    planning_scene_publisher_->publish(planning_scene);
+    _planning_scene_publisher->publish(planning_scene);
-    RCLCPP_INFO(this->get_logger(), "Published collision objects: table and mesh");
+    RCLCPP_INFO(this->get_logger(),
-    
+                "Published collision objects: table and mesh");
+
     object_published_ = true;
   }
 
-  shape_msgs::msg::Mesh getMeshCollisionObject(const std::string &mesh_filepath) {
+  shape_msgs::msg::Mesh
+  getMeshCollisionObject(const std::string &mesh_filepath) {
     shape_msgs::msg::Mesh mesh;
-    shapes::Mesh* m = shapes::createMeshFromResource(mesh_filepath);
+    shapes::Mesh *m = shapes::createMeshFromResource(mesh_filepath);
     shapes::ShapeMsg mesh_msg;
     shapes::constructMsgFromShape(m, mesh_msg);
     mesh = boost::get<shape_msgs::msg::Mesh>(mesh_msg);