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

update grasping poses
This commit is contained in:
Ilya Uraev 2025-03-17 14:15:41 +03:00
parent 6c5cf0633c
commit 9b9c706c97
7 changed files with 121 additions and 8 deletions

View file

@ -28,6 +28,9 @@ list(APPEND plugin_libs get_named_pose)
add_library(is_in_pose SHARED plugins/is_in_pose.cpp) add_library(is_in_pose SHARED plugins/is_in_pose.cpp)
list(APPEND plugin_libs is_in_pose) 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}) foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies}) ament_target_dependencies(${bt_plugin} ${dependencies})
target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

View file

@ -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");

View file

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

View file

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

View file

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

View file

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

View file

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