refactor: using relative positions in config files

rbs_mill_assist/config/key_poses.yaml

@@ -1,10 +0,0 @@
-waiting:
-  position:
-    x: 0.23034882653403538
-    y: -0.1912536308604868
-    z: 0.13099884470917866
-  orientation:
-    x: 1.0
-    y: 0.0
-    z: 0.0
-    w: 0.0

rbs_mill_assist/moveit/chomp_planning.yaml

@@ -1,37 +0,0 @@
-planning_plugins:
-  - chomp_interface/CHOMPPlanner
-request_adapters:
-  - default_planning_request_adapters/ResolveConstraintFrames
-  - default_planning_request_adapters/ValidateWorkspaceBounds
-  - default_planning_request_adapters/CheckStartStateBounds
-  - default_planning_request_adapters/CheckStartStateCollision
-response_adapters:
-  - default_planning_response_adapters/AddTimeOptimalParameterization
-planning_time_limit: 10.0
-max_iterations: 200
-max_iterations_after_collision_free: 5
-smoothness_cost_weight: 0.1
-obstacle_cost_weight: 1.0
-learning_rate: 0.01
-animate_path: true
-add_randomness: false
-smoothness_cost_velocity: 0.0
-smoothness_cost_acceleration: 1.0
-smoothness_cost_jerk: 0.0
-hmc_discretization: 0.01
-hmc_stochasticity: 0.01
-hmc_annealing_factor: 0.99
-use_hamiltonian_monte_carlo: false
-ridge_factor: 0.0
-use_pseudo_inverse: false
-pseudo_inverse_ridge_factor: 1e-4
-animate_endeffector: false
-# animate_endeffector_segment: "panda_rightfinger"
-joint_update_limit: 0.1
-collision_clearance: 0.2
-collision_threshold: 0.07
-random_jump_amount: 1.0
-use_stochastic_descent: true
-enable_failure_recovery: false
-max_recovery_attempts: 5
-trajectory_initialization_method: "quintic-spline"

rbs_mill_assist/src/places_frame_publisher.cpp

@@ -1,5 +1,7 @@
+#include <chrono>
 #include <ctime>
 #include <memory>
+#include <rclcpp/executors/single_threaded_executor.hpp>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
@@ -57,7 +59,7 @@ private:
       geometry_msgs::msg::TransformStamped t;
 
       t.header.stamp = this->get_clock()->now();
-      t.header.frame_id = "world";
+      t.header.frame_id = places_config->getModelParents()[i];
       t.child_frame_id = places_config->getModelNames()[i];
 
       geometry_msgs::msg::Pose tmp_pose;
@@ -80,7 +82,10 @@ private:
 
 int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
-  rclcpp::spin(std::make_shared<PlacesFramePublisher>());
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<PlacesFramePublisher>();
+  executor.add_node(node);
+  executor.spin();
   rclcpp::shutdown();
   return 0;
 }

rbs_mill_assist/src/planning_scene_publisher.cpp

@@ -6,6 +6,9 @@
 #include <chrono>
 #include <functional>
 #include <geometry_msgs/msg/pose.hpp>
+#include <memory>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/executors/single_threaded_executor.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -16,10 +19,13 @@
 #include <moveit_msgs/msg/planning_scene.hpp>
 #include <shape_msgs/msg/mesh.hpp>
 #include <shape_msgs/msg/solid_primitive.hpp>
+#include <tf2/time.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
-class PlanningScenePublisher : public rclcpp::Node {
+class PlanningSceneNode : public rclcpp::Node {
 public:
-  PlanningScenePublisher()
+  PlanningSceneNode()
       : Node("planning_scene_publisher"), object_published_(false) {
     this->declare_parameter("places_config", "");
     _planning_scene_publisher =
@@ -27,17 +33,20 @@ public:
             "planning_scene", 10);
     _timer = this->create_wall_timer(
         std::chrono::seconds(1),
-        std::bind(&PlanningScenePublisher::publish_scene, this));
+        std::bind(&PlanningSceneNode::publish_scene, this));
     auto places_config_filepath =
         this->get_parameter("places_config").as_string();
 
+    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+
+
     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:
@@ -47,6 +56,9 @@ private:
   bool object_published_;
   std::shared_ptr<PlacesConfig> _places_config;
 
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+
   void publish_scene() {
     if (object_published_) {
       return;
@@ -73,20 +85,24 @@ private:
 
     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";
-
     for (size_t i = 0; i < _places_config->getMeshFilepaths().size(); i++) {
+      if (!tf_buffer_->canTransform(_places_config->getModelParents()[i],
+                                    _places_config->getModelNames()[i],
+                                    tf2::TimePointZero)) {
+        RCLCPP_WARN(this->get_logger(), "Waiting for TF frame '%s'...",
+                    _places_config->getModelParents()[i].c_str());
+        return;
+      }
+      moveit_msgs::msg::CollisionObject mesh_object;
+      mesh_object.id = _places_config->getModelNames()[i];
+      mesh_object.header.frame_id = _places_config->getModelParents()[i];
       auto mesh = getMeshCollisionObject(_places_config->getMeshFilepaths()[i]);
       mesh_object.meshes.push_back(mesh);
+      mesh_object.mesh_poses.push_back(_places_config->getMeshPoses()[i]);
+      mesh_object.operation = moveit_msgs::msg::CollisionObject::ADD;
+      planning_scene.world.collision_objects.push_back(mesh_object);
     }
 
-    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);
@@ -105,28 +121,14 @@ private:
     mesh = boost::get<shape_msgs::msg::Mesh>(mesh_msg);
     return mesh;
   }
-
-  geometry_msgs::msg::Pose arrayToPose(const std::vector<double> arr) {
-    if (arr.size() != 7) {
-      throw std::invalid_argument("Size of array must be equal 7");
-    }
-    geometry_msgs::msg::Pose pose;
-    pose.position.x = arr[0];
-    pose.position.y = arr[1];
-    pose.position.z = arr[2];
-
-    pose.orientation.x = arr[3];
-    pose.orientation.y = arr[4];
-    pose.orientation.z = arr[5];
-    pose.orientation.w = arr[6];
-    return pose;
-  }
 };
 
 int main(int argc, char **argv) {
   rclcpp::init(argc, argv);
-  auto node = std::make_shared<PlanningScenePublisher>();
-  rclcpp::spin(node);
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<PlanningSceneNode>();
+  executor.add_node(node);
+  executor.spin();
   rclcpp::shutdown();
   return 0;
 }

rbs_mill_assist/world/config/places.yaml

@@ -77,19 +77,15 @@ models:
     mesh_filename: conductor.stl
     pose:
       position:
-        x: 0.350
+        x: 0.220
         y: -0.170
         z: 0.01
-      # orientation:
-      #   r: 0.0
-      #   p: 0.0
-      #   y: 0.0
 
   - name: laser
     mesh_filename: laser.dae
     pose:
       position:
-        x: 0.350
+        x: 0.250
         y: -0.170
         z: 0.0
       orientation:

rbs_mill_assist/world/config/ref_frames.yaml

@@ -0,0 +1,149 @@
+ref_frames:
+  - name: "slot_1"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: 0.066
+        z: 0.0
+  - name: "slot_2"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: 0.022
+        z: 0.0
+  - name: "slot_3"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: -0.022
+        z: 0.0
+  - name: "slot_4"
+    relative_to: bunker_4_slots
+    pose:
+      position:
+        x: 0.0
+        y: -0.066
+        z: 0.0
+
+  - name: waiting
+    relative_to: conductor
+    pose:
+      position:
+        x: -0.22
+        y: 0.19
+        z: 0.13
+      orientation:
+        r: 3.14
+        p: 0.0
+        y: 0.0
+
+  - name: pregrasp_1
+    relative_to: slot_1
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.1
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: grasp_1
+    relative_to: slot_1
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.02
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: pregrasp_2
+    relative_to: slot_2
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.1
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: pregrasp_3
+    relative_to: slot_3
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.1
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: pregrasp_4
+    relative_to: slot_4
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.1
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: preplace_laser
+    relative_to: conductor
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.1
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: place_laser
+    relative_to: conductor
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.02
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: preplace_out
+    relative_to: bunker
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.1
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57
+
+  - name: place_out
+    relative_to: bunker
+    pose:
+      position:
+        x: 0.0
+        y: 0.0
+        z: 0.02
+      orientation:
+        r: 0.0
+        p: 3.14
+        y: 1.57

rbs_mill_assist/world/config/slots.yaml

@@ -1,29 +0,0 @@
-ref_frames:
-  - name: "slot_1"
-    relative_to: bunker_4_slots
-    pose:
-      position:
-        x: 0.0
-        y: 0.066
-        z: 0.0
-  - name: "slot_2"
-    relative_to: bunker_4_slots
-    pose:
-      position:
-        x: 0.0
-        y: 0.022
-        z: 0.0
-  - name: "slot_3"
-    relative_to: bunker_4_slots
-    pose:
-      position:
-        x: 0.0
-        y: -0.022
-        z: 0.0
-  - name: "slot_4"
-    relative_to: bunker_4_slots
-    pose:
-      position:
-        x: 0.0
-        y: -0.066
-        z: 0.0