diff --git a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp
index 7eeb671..f8a2a95 100644
--- a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp
+++ b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp
@@ -34,11 +34,12 @@ public:
   }
 
   bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
-                this->service_name_.c_str());
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
     if (!getInput("action_name", request->action_name)) {
       RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Failed to get object_name from input port");
+                   "[%s] Failed to get object_name from input port",
+                   name().c_str());
       return false;
     }
     return true;
@@ -47,25 +48,25 @@ public:
   NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
     if (!response->ok) {
       RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Response indicates failure.");
+                   "[%s] Response indicates failure.", name().c_str());
       return NodeStatus::FAILURE;
     }
 
     RCLCPP_INFO(this->node_.lock()->get_logger(),
-                "Response received successfully.");
+                "[%s] Response received successfully.", name().c_str());
 
     auto logPose = [this](const std::string &pose_name,
                           const geometry_msgs::msg::Pose &pose) {
       RCLCPP_INFO(this->node_.lock()->get_logger(),
-                  "%s:\n"
+                  "[%s] %s:\n"
                   "  Position: x = %.4f, y = %.4f, z = %.4f\n"
                   "  Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f",
-                  pose_name.c_str(), pose.position.x, pose.position.y,
-                  pose.position.z, pose.orientation.x, pose.orientation.y,
-                  pose.orientation.z, pose.orientation.w);
+                  name().c_str(), pose_name.c_str(), pose.position.x,
+                  pose.position.y, pose.position.z, pose.orientation.x,
+                  pose.orientation.y, pose.orientation.z, pose.orientation.w);
     };
     if (!response->grasp.empty()) {
-      RCLCPP_INFO(this->logger(), "Got Pick Response");
+      RCLCPP_INFO(this->logger(), "[%s] Got Pick Response", name().c_str());
       std::vector<std::string> poses = {"Pregrasp Pose", "Grasp Pose",
                                         "Postgrasp Pose"};
 
@@ -87,7 +88,7 @@ public:
       return NodeStatus::SUCCESS;
     }
     if (!response->place.empty()) {
-      RCLCPP_INFO(this->logger(), "Got Place Response");
+      RCLCPP_INFO(this->logger(), "[%s] Got Place Response", name().c_str());
       std::vector<std::string> poses = {"Preplace Pose", "Place Pose",
                                         "Postplace Pose"};
 
@@ -109,7 +110,7 @@ public:
       return NodeStatus::SUCCESS;
     }
 
-    RCLCPP_FATAL(this->logger(), "Could not response grasp pose");
+    RCLCPP_FATAL(this->logger(), "[%s] Could not response grasp pose", name().c_str());
     return NodeStatus::FAILURE;
   }
   // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
diff --git a/rbs_mill_assist/bt/plugins/get_named_pose.cpp b/rbs_mill_assist/bt/plugins/get_named_pose.cpp
index 3b50b25..e14ad5d 100644
--- a/rbs_mill_assist/bt/plugins/get_named_pose.cpp
+++ b/rbs_mill_assist/bt/plugins/get_named_pose.cpp
@@ -31,11 +31,12 @@ public:
   }
 
   bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "Starting send request for: [%s]",
-                this->service_name_.c_str());
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]",
+                name().c_str(), this->service_name_.c_str());
     if (!getInput("pose_name", request->pose_name)) {
       RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Failed to get pose_name from input port");
+                   "[%s] Failed to get pose_name from input port",
+                   name().c_str());
       return false;
     }
     return true;
@@ -43,13 +44,14 @@ public:
 
   NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
     if (!response->ok) {
-      RCLCPP_ERROR(this->logger(), "Response indicates failure.");
+      RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.",
+                   name().c_str());
       return NodeStatus::FAILURE;
     }
 
     RCLCPP_INFO(this->logger(),
-                "Response received successfully with pose name [%s]",
-                response->named_pose.name.c_str());
+                "[%s] Response received successfully with pose name [%s]",
+                name().c_str(), response->named_pose.name.c_str());
 
     auto pose = std::make_shared<geometry_msgs::msg::Pose>();
     *pose = response->named_pose.pose;
diff --git a/rbs_mill_assist/bt/plugins/is_in_pose.cpp b/rbs_mill_assist/bt/plugins/is_in_pose.cpp
new file mode 100644
index 0000000..dda3714
--- /dev/null
+++ b/rbs_mill_assist/bt/plugins/is_in_pose.cpp
@@ -0,0 +1,48 @@
+#include "behaviortree_ros2/bt_topic_sub_node.hpp"
+
+#include "behaviortree_ros2/plugins.hpp"
+#include <behaviortree_cpp/basic_types.h>
+// #include <geometry_msgs/msg/detail/point__struct.hpp>
+#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
+#include "rbs_utils_interfaces/srv/get_named_pose.hpp"
+#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
+#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
+#include <memory>
+#include <rclcpp/logging.hpp>
+#include <string>
+
+using MSG = geometry_msgs::msg::Pose;
+using namespace BT;
+
+class IsInPose : public RosTopicSubNode<MSG> {
+public:
+  IsInPose(const std::string &name, const NodeConfig &conf,
+           const RosNodeParams &params)
+      : RosTopicSubNode<MSG>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting IsInPose");
+  }
+  //
+  // static PortsList providedPorts() {
+  //   return {};
+  // }
+
+  NodeStatus onTick(const std::shared_ptr<MSG> &t_last_msg) override {
+    if (t_last_msg) {
+      RCLCPP_INFO(this->logger(), "[%s] Got message Pose", name().c_str());
+      m_last_msg = t_last_msg;
+      return NodeStatus::SUCCESS;
+    }
+    RCLCPP_INFO(this->logger(), "[%s] NOT message received", name().c_str());
+    return NodeStatus::SUCCESS;
+  }
+  bool latchLastMessage() const override {
+    return true;
+  }
+
+private:
+  MSG::SharedPtr m_last_msg;
+};
+
+CreateRosNodePlugin(IsInPose, "IsInPose");
diff --git a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp
index a7a2fb1..5bb5c04 100644
--- a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp
+++ b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp
@@ -24,10 +24,10 @@ public:
   }
 
   bool setRequest(Request::SharedPtr &request) override {
-    RCLCPP_INFO(this->logger(), "Starting send request");
+    RCLCPP_INFO(this->logger(), "[%s] Starting send request", name().c_str());
     if (!getInput("enable", request->data)) {
-      RCLCPP_ERROR(this->node_.lock()->get_logger(),
-                   "Failed to get sending data from input port");
+      RCLCPP_ERROR(this->logger(),
+                   "[%s] Failed to get sending data from input port", name().c_str());
       return false;
     }
     return true;
@@ -35,7 +35,7 @@ public:
 
   NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
     if (!response->success) {
-      RCLCPP_ERROR(this->logger(), "Response indicates failure.");
+      RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.", name().c_str());
       return NodeStatus::FAILURE;
     }