add twist cmd with condition BT plugin and refactor VacuumGripper to publish contact info

update grasping poses

rbs_mill_assist/bt/CMakeLists.txt

@@ -28,6 +28,9 @@ list(APPEND plugin_libs get_named_pose)
 add_library(is_in_pose SHARED plugins/is_in_pose.cpp)
 list(APPEND plugin_libs is_in_pose)
 
+add_library(twist_command_with_condition SHARED plugins/twist_cmd.cpp)
+list(APPEND plugin_libs twist_command_with_condition)
+
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})
     target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

rbs_mill_assist/bt/plugins/twist_cmd.cpp

@@ -0,0 +1,85 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "rbs_skill_interfaces/action/twist_control_with_condition.hpp"
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+#include <rclcpp/logging.hpp>
+
+using namespace BT;
+using TwistCmdWithCondAction =
+    rbs_skill_interfaces::action::TwistControlWithCondition;
+
+class TwistCmdWithCondClient : public RosActionNode<TwistCmdWithCondAction> {
+public:
+  TwistCmdWithCondClient(const std::string &name, const NodeConfig &conf,
+                         const RosNodeParams &params)
+      : RosActionNode<TwistCmdWithCondAction>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting MoveToPose");
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts({BT::InputPort<std::string>("robot_name"),
+                               BT::InputPort<std::vector<double>>("twist"),
+                               // BT::InputPort<std::string>("condition_topic")
+    });
+  }
+
+  bool setGoal(RosActionNode::Goal &goal) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
+                this->action_name_.c_str());
+
+    if (!getInput("robot_name", goal.robot_name)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get robot name",
+                   name().c_str());
+      return false;
+    }
+
+    if (!getInput("twist", twist_cmd_vec)) {
+      RCLCPP_FATAL(this->logger(), "[%s] Could not get twist command",
+                   name().c_str());
+      return false;
+    }
+    if (!twist_cmd_vec.empty() && twist_cmd_vec.size() == 6) {
+      goal.twist_cmd.linear.x = twist_cmd_vec[0];
+      goal.twist_cmd.linear.y = twist_cmd_vec[1];
+      goal.twist_cmd.linear.z = twist_cmd_vec[2];
+      goal.twist_cmd.angular.x = twist_cmd_vec[3];
+      goal.twist_cmd.angular.y = twist_cmd_vec[4];
+      goal.twist_cmd.angular.z = twist_cmd_vec[5];
+    } else {
+      RCLCPP_FATAL(this->logger(),
+                   "[%s] Twist command is empty or should be 1x6 [x;y;z;r;p;y]",
+                   name().c_str());
+      return false;
+    }
+
+    // if (!getInput("condition_topic", goal.condition_topic)) {
+    //   RCLCPP_FATAL(this->logger(), "[%s] Could not get condition_topic",
+    //                name().c_str());
+    //   return false;
+    // } else {
+    //   RCLCPP_INFO(this->logger(), "[%s] Using condition_topic [%s]",
+    //               name().c_str(), goal.condition_topic.c_str());
+    // }
+
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
+
+    RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b",
+                name().c_str(), this->action_name_.c_str(), wr.result->success);
+    if (!wr.result->success) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  // std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
+  std::vector<double> twist_cmd_vec;
+};
+
+CreateRosNodePlugin(TwistCmdWithCondClient, "TwistCmdWithCond");

rbs_mill_assist/config/grasping.yaml

@@ -44,7 +44,7 @@ to_graver:
     position:
       x: 0.40
       y: 0.0
-      z: 0.01
+      z: 0.015
     orientation:
       x: 0.707
       y: 0.707
@@ -76,7 +76,7 @@ from_graver:
     position:
       x: 0.40
       y: 0.0
-      z: 0.01
+      z: 0.015
     orientation:
       x: 0.707
       y: 0.707

rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp

@@ -8,7 +8,9 @@
 #include <gz/sim/System.hh>
 
 #include "std_srvs/srv/set_bool.hpp"
+#include "std_msgs/msg/bool.hpp"
 #include <memory>
+#include <rclcpp/publisher.hpp>
 #include <rclcpp/service.hpp>
 
 namespace rbs_mill_assist {
@@ -17,7 +19,8 @@ class VacuumGripperPrivate;
 
 class VacuumGripper : public gz::sim::System,
                       public gz::sim::ISystemConfigure,
-                      public gz::sim::ISystemPreUpdate {
+                      public gz::sim::ISystemPreUpdate,
+                      public gz::sim::ISystemPostUpdate {
 public:
   VacuumGripper();
   ~VacuumGripper() override;
@@ -29,6 +32,9 @@ public:
   void PreUpdate(const gz::sim::UpdateInfo &_info,
                  gz::sim::EntityComponentManager &_ecm) override;
 
+  void PostUpdate(const gz::sim::UpdateInfo &_info,
+                  const gz::sim::EntityComponentManager &_ecm) override;
+
   bool AttachLink(gz::sim::EntityComponentManager &_ecm,
                   const gz::sim::Entity &gripperEntity,
                   const gz::sim::Entity &objectEntity);
@@ -41,6 +47,7 @@ public:
 private:
   std::unique_ptr<VacuumGripperPrivate> dataPtr;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_service_;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr contact_publisher_;
 };
 } // namespace rbs_mill_assist
 #endif

rbs_mill_assist/src/CMakeLists.txt

@@ -3,6 +3,7 @@ find_package(rclcpp REQUIRED)
 find_package(gz-cmake3 REQUIRED)
 find_package(gz-plugin2 REQUIRED COMPONENTS register)
 find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
 set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
 find_package(gz-common5 REQUIRED COMPONENTS profiler)
 set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
@@ -31,6 +32,7 @@ target_link_libraries(VacuumGripper
 ament_target_dependencies(VacuumGripper
   rclcpp
   std_srvs
+  std_msgs
 )
 
 install(

rbs_mill_assist/src/VacuumGripper.cpp

@@ -1,4 +1,5 @@
 #include "gz/sim/components/Name.hh"
+#include "std_msgs/msg/bool.hpp"
 #include <functional>
 #include <gz/common/Console.hh>
 #include <gz/plugin/Register.hh>
@@ -21,7 +22,8 @@
 
 GZ_ADD_PLUGIN(rbs_mill_assist::VacuumGripper, gz::sim::System,
               rbs_mill_assist::VacuumGripper::ISystemConfigure,
-              rbs_mill_assist::VacuumGripper::ISystemPreUpdate)
+              rbs_mill_assist::VacuumGripper::ISystemPreUpdate,
+              rbs_mill_assist::VacuumGripper::ISystemPostUpdate)
 
 namespace rbs_mill_assist {
 
@@ -31,6 +33,7 @@ public:
   bool attach_request;
   bool detach_request;
   bool enabled;
+  bool attached;
   gz::sim::EntityComponentManager _ecm;
 
   gz::sim::components::Joint joint;
@@ -88,6 +91,7 @@ bool VacuumGripper::AttachLink(gz::sim::EntityComponentManager &_ecm,
         << std::endl;
 
   this->dataPtr->contactedEntity = objectEntity;
+  this->dataPtr->attached = true;
 
   return true;
 }
@@ -113,6 +117,7 @@ bool VacuumGripper::DetachLink(gz::sim::EntityComponentManager &_ecm) {
 
   this->dataPtr->contactedEntity = gz::sim::kNullEntity;
   this->dataPtr->detachableJoints.erase(it);
+  this->dataPtr->attached = false;
 
   return true;
 }
@@ -180,6 +185,10 @@ void VacuumGripper::Configure(
           std::bind(&VacuumGripper::Toggle, this, std::placeholders::_1,
                     std::placeholders::_2));
 
+  contact_publisher_ =
+      this->dataPtr->node_->create_publisher<std_msgs::msg::Bool>(
+          node_name + "/contact_info", 10);
+
   gzdbg << "rbs_mill_assist::VacuumGripper::Configure on entity: " << _entity
         << std::endl;
 }
@@ -234,4 +243,11 @@ void VacuumGripper::PreUpdate(const gz::sim::UpdateInfo &_info,
       });
 }
 
+void VacuumGripper::PostUpdate(const gz::sim::UpdateInfo &_info,
+                               const gz::sim::EntityComponentManager &_ecm) {
+  std_msgs::msg::Bool msg;
+  msg.data = this->dataPtr->attached;
+  contact_publisher_->publish(msg);
+}
+
 } // namespace rbs_mill_assist

rbs_mill_assist/urdf/rbs_arm_macro.xacro

@@ -278,8 +278,8 @@
       <axis
         xyz="0 0 1" />
       <limit
-        lower="-3.14159"
+        lower="-6.2832"
-        upper="3.14159"
+        upper="6.2832"
         effort="30"
         velocity="10" />
     </joint>
@@ -394,8 +394,8 @@
       <axis
         xyz="0 0 1" />
       <limit
-        lower="-3.14159"
+        lower="-6.2832"
-        upper="3.14159"
+        upper="6.2832"
         effort="30"
         velocity="10" />
     </joint>