From 9b9c706c974545c55100ce2536858e1013f1459f Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 17 Mar 2025 14:15:41 +0300 Subject: [PATCH] add twist cmd with condition BT plugin and refactor VacuumGripper to publish contact info update grasping poses --- rbs_mill_assist/bt/CMakeLists.txt | 3 + rbs_mill_assist/bt/plugins/twist_cmd.cpp | 85 +++++++++++++++++++ rbs_mill_assist/config/grasping.yaml | 4 +- .../include/rbs_mill_assist/VacuumGripper.hpp | 9 +- rbs_mill_assist/src/CMakeLists.txt | 2 + rbs_mill_assist/src/VacuumGripper.cpp | 18 +++- rbs_mill_assist/urdf/rbs_arm_macro.xacro | 8 +- 7 files changed, 121 insertions(+), 8 deletions(-) create mode 100644 rbs_mill_assist/bt/plugins/twist_cmd.cpp diff --git a/rbs_mill_assist/bt/CMakeLists.txt b/rbs_mill_assist/bt/CMakeLists.txt index e02037e..1d6a06b 100644 --- a/rbs_mill_assist/bt/CMakeLists.txt +++ b/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) diff --git a/rbs_mill_assist/bt/plugins/twist_cmd.cpp b/rbs_mill_assist/bt/plugins/twist_cmd.cpp new file mode 100644 index 0000000..7688e56 --- /dev/null +++ b/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 +#include +#include +#include + +using namespace BT; +using TwistCmdWithCondAction = + rbs_skill_interfaces::action::TwistControlWithCondition; + +class TwistCmdWithCondClient : public RosActionNode { +public: + TwistCmdWithCondClient(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosActionNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting MoveToPose"); + } + + static BT::PortsList providedPorts() { + return providedBasicPorts({BT::InputPort("robot_name"), + BT::InputPort>("twist"), + // BT::InputPort("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 m_target_pose; + std::vector twist_cmd_vec; +}; + +CreateRosNodePlugin(TwistCmdWithCondClient, "TwistCmdWithCond"); diff --git a/rbs_mill_assist/config/grasping.yaml b/rbs_mill_assist/config/grasping.yaml index f0d8f7c..76cb803 100644 --- a/rbs_mill_assist/config/grasping.yaml +++ b/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 diff --git a/rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp b/rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp index f49cb27..eef0028 100644 --- a/rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp +++ b/rbs_mill_assist/include/rbs_mill_assist/VacuumGripper.hpp @@ -8,7 +8,9 @@ #include #include "std_srvs/srv/set_bool.hpp" +#include "std_msgs/msg/bool.hpp" #include +#include #include 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 dataPtr; rclcpp::Service::SharedPtr toggle_service_; + rclcpp::Publisher::SharedPtr contact_publisher_; }; } // namespace rbs_mill_assist #endif diff --git a/rbs_mill_assist/src/CMakeLists.txt b/rbs_mill_assist/src/CMakeLists.txt index 837f15d..f436668 100644 --- a/rbs_mill_assist/src/CMakeLists.txt +++ b/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( diff --git a/rbs_mill_assist/src/VacuumGripper.cpp b/rbs_mill_assist/src/VacuumGripper.cpp index d97d1d9..2961bd3 100644 --- a/rbs_mill_assist/src/VacuumGripper.cpp +++ b/rbs_mill_assist/src/VacuumGripper.cpp @@ -1,4 +1,5 @@ #include "gz/sim/components/Name.hh" +#include "std_msgs/msg/bool.hpp" #include #include #include @@ -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( + 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 diff --git a/rbs_mill_assist/urdf/rbs_arm_macro.xacro b/rbs_mill_assist/urdf/rbs_arm_macro.xacro index ab2bff6..8ef4316 100644 --- a/rbs_mill_assist/urdf/rbs_arm_macro.xacro +++ b/rbs_mill_assist/urdf/rbs_arm_macro.xacro @@ -278,8 +278,8 @@ @@ -394,8 +394,8 @@