add VacuumGripper plugin
This commit is contained in:
parent
a3397f6686
commit
ae418084dd
8 changed files with 393 additions and 42 deletions
|
|
@ -7,10 +7,46 @@ endif()
|
|||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_srvs REQUIRED)
|
||||
find_package(gz-cmake3 REQUIRED)
|
||||
find_package(gz-plugin2 REQUIRED COMPONENTS register)
|
||||
set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
|
||||
find_package(gz-common5 REQUIRED COMPONENTS profiler)
|
||||
set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
|
||||
|
||||
# Harmonic
|
||||
if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
|
||||
find_package(gz-sim8 REQUIRED)
|
||||
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
|
||||
message(STATUS "Compiling against Gazebo Harmonic")
|
||||
# Default to Garden
|
||||
else()
|
||||
find_package(gz-sim7 REQUIRED)
|
||||
set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
|
||||
message(STATUS "Compiling against Gazebo Garden")
|
||||
endif()
|
||||
|
||||
add_library(VacuumGripper
|
||||
SHARED
|
||||
src/VacuumGripper.cpp
|
||||
)
|
||||
|
||||
include_directories(include)
|
||||
|
||||
target_link_libraries(VacuumGripper
|
||||
gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
|
||||
)
|
||||
|
||||
ament_target_dependencies(VacuumGripper
|
||||
rclcpp
|
||||
std_srvs
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS VacuumGripper
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(DIRECTORY launch urdf config world assets
|
||||
DESTINATION share/${PROJECT_NAME})
|
||||
|
|
|
|||
46
include/rbs_mill_assist/VacuumGripper.hpp
Normal file
46
include/rbs_mill_assist/VacuumGripper.hpp
Normal file
|
|
@ -0,0 +1,46 @@
|
|||
#ifndef RBS_MILL_ASSIST__FULL_SYSTEM_HH_
|
||||
#define RBS_MILL_ASSIST__FULL_SYSTEM_HH_
|
||||
|
||||
// The only required include in the header is this one.
|
||||
// All others will depend on what your plugin does.
|
||||
#include "gz/sim/components/Factory.hh"
|
||||
#include <gz/sim/EventManager.hh>
|
||||
#include <gz/sim/System.hh>
|
||||
|
||||
#include "std_srvs/srv/set_bool.hpp"
|
||||
#include <memory>
|
||||
#include <rclcpp/service.hpp>
|
||||
|
||||
namespace rbs_mill_assist {
|
||||
|
||||
class VacuumGripperPrivate;
|
||||
|
||||
class VacuumGripper : public gz::sim::System,
|
||||
public gz::sim::ISystemConfigure,
|
||||
public gz::sim::ISystemPreUpdate {
|
||||
public:
|
||||
VacuumGripper();
|
||||
~VacuumGripper() override;
|
||||
void Configure(const gz::sim::Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_element,
|
||||
gz::sim::EntityComponentManager &_ecm,
|
||||
gz::sim::EventManager &_eventManager) override;
|
||||
|
||||
void PreUpdate(const gz::sim::UpdateInfo &_info,
|
||||
gz::sim::EntityComponentManager &_ecm) override;
|
||||
|
||||
bool AttachLink(gz::sim::EntityComponentManager &_ecm,
|
||||
const gz::sim::Entity &gripperEntity,
|
||||
const gz::sim::Entity &objectEntity);
|
||||
|
||||
bool DetachLink(gz::sim::EntityComponentManager &_ecm);
|
||||
|
||||
void Toggle(const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response> response);
|
||||
|
||||
private:
|
||||
std::unique_ptr<VacuumGripperPrivate> dataPtr;
|
||||
rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr toggle_service_;
|
||||
};
|
||||
} // namespace rbs_mill_assist
|
||||
#endif
|
||||
|
|
@ -143,7 +143,7 @@ def launch_setup(context, *args, **kwargs):
|
|||
PythonLaunchDescriptionSource(
|
||||
[os.path.join(get_package_share_directory('ros_gz_sim'),
|
||||
'launch', 'gz_sim.launch.py')]),
|
||||
launch_arguments=[('gz_args', [' -r ', gazebo_world])],
|
||||
launch_arguments=[('gz_args', [' -r -v 4 ', gazebo_world])],
|
||||
)
|
||||
|
||||
gazebo_spawn_robot = Node(
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@
|
|||
|
||||
<depend>rbs_bringup</depend>
|
||||
<depend>rbs_arm</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>cartesian_compliance_controller</depend>
|
||||
<depend>cartesian_controller_base</depend>
|
||||
<depend>cartesian_controller_handles</depend>
|
||||
|
|
|
|||
237
src/VacuumGripper.cpp
Normal file
237
src/VacuumGripper.cpp
Normal file
|
|
@ -0,0 +1,237 @@
|
|||
#include "gz/sim/components/Name.hh"
|
||||
#include <functional>
|
||||
#include <gz/common/Console.hh>
|
||||
#include <gz/plugin/Register.hh>
|
||||
#include <gz/sim/Entity.hh>
|
||||
#include <gz/sim/EntityComponentManager.hh>
|
||||
#include <gz/sim/Model.hh>
|
||||
|
||||
#include <gz/sim/components/ChildLinkName.hh>
|
||||
#include <gz/sim/components/Collision.hh>
|
||||
#include <gz/sim/components/ContactSensorData.hh>
|
||||
#include <gz/sim/components/DetachableJoint.hh>
|
||||
#include <gz/sim/components/Joint.hh>
|
||||
#include <gz/sim/components/ParentEntity.hh>
|
||||
|
||||
#include <memory>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <string>
|
||||
|
||||
#include "rbs_mill_assist/VacuumGripper.hpp"
|
||||
|
||||
GZ_ADD_PLUGIN(rbs_mill_assist::VacuumGripper, gz::sim::System,
|
||||
rbs_mill_assist::VacuumGripper::ISystemConfigure,
|
||||
rbs_mill_assist::VacuumGripper::ISystemPreUpdate)
|
||||
|
||||
namespace rbs_mill_assist {
|
||||
|
||||
class VacuumGripperPrivate {
|
||||
public:
|
||||
std::string gripper_link_name;
|
||||
bool attach_request;
|
||||
bool detach_request;
|
||||
bool enabled;
|
||||
gz::sim::EntityComponentManager _ecm;
|
||||
|
||||
gz::sim::components::Joint joint;
|
||||
gz::sim::Entity contactedEntity = gz::sim::kNullEntity;
|
||||
gz::sim::Entity gripperEntity = gz::sim::kNullEntity;
|
||||
std::unordered_map<std::string, gz::sim::Entity> detachableJoints;
|
||||
|
||||
std::shared_ptr<rclcpp::Node> node_{nullptr};
|
||||
std::thread thread_executor_spin_;
|
||||
rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
|
||||
};
|
||||
|
||||
VacuumGripper::VacuumGripper()
|
||||
: dataPtr(std::make_unique<VacuumGripperPrivate>()) {}
|
||||
|
||||
VacuumGripper::~VacuumGripper() {
|
||||
this->dataPtr->executor_->cancel();
|
||||
this->dataPtr->thread_executor_spin_.join();
|
||||
}
|
||||
|
||||
void VacuumGripper::Toggle(
|
||||
const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
|
||||
std::shared_ptr<std_srvs::srv::SetBool::Response> response) {
|
||||
this->dataPtr->enabled = request->data;
|
||||
response->success = true;
|
||||
gzdbg << "Vacuum Gripper " << (request->data ? "Activated" : "Deactivated")
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
bool VacuumGripper::AttachLink(gz::sim::EntityComponentManager &_ecm,
|
||||
const gz::sim::Entity &gripperEntity,
|
||||
const gz::sim::Entity &objectEntity) {
|
||||
|
||||
if (gripperEntity == gz::sim::kNullEntity ||
|
||||
objectEntity == gz::sim::kNullEntity) {
|
||||
gzerr << "Invalid gripper or object entity!" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
auto jointEntity = _ecm.CreateEntity();
|
||||
|
||||
gz::sim::components::DetachableJointInfo jointInfo;
|
||||
jointInfo.parentLink = gripperEntity;
|
||||
jointInfo.childLink = objectEntity;
|
||||
jointInfo.jointType = "fixed";
|
||||
|
||||
_ecm.CreateComponent(jointEntity,
|
||||
gz::sim::components::DetachableJoint(jointInfo));
|
||||
|
||||
std::string key =
|
||||
std::to_string(gripperEntity) + "_" + std::to_string(objectEntity);
|
||||
this->dataPtr->detachableJoints[key] = jointEntity;
|
||||
|
||||
gzdbg << "Attached object " << objectEntity << " to gripper " << gripperEntity
|
||||
<< std::endl;
|
||||
|
||||
this->dataPtr->contactedEntity = objectEntity;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool VacuumGripper::DetachLink(gz::sim::EntityComponentManager &_ecm) {
|
||||
if (this->dataPtr->contactedEntity == gz::sim::kNullEntity) {
|
||||
gzdbg << "No object is currently attached to the gripper." << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string key = std::to_string(this->dataPtr->gripperEntity) + "_" +
|
||||
std::to_string(this->dataPtr->contactedEntity);
|
||||
|
||||
auto it = this->dataPtr->detachableJoints.find(key);
|
||||
if (it == this->dataPtr->detachableJoints.end()) {
|
||||
gzerr << "Failed to find detachable joint for object "
|
||||
<< this->dataPtr->contactedEntity << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
auto jointEntity = it->second;
|
||||
_ecm.RequestRemoveEntity(jointEntity);
|
||||
|
||||
this->dataPtr->contactedEntity = gz::sim::kNullEntity;
|
||||
this->dataPtr->detachableJoints.erase(it);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void VacuumGripper::Configure(
|
||||
const gz::sim::Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_element,
|
||||
gz::sim::EntityComponentManager &_ecm,
|
||||
gz::sim::EventManager &_eventManager) {
|
||||
|
||||
rclcpp::Logger logger = rclcpp::get_logger("VacuumGripperGazeboPlugin");
|
||||
const auto model = gz::sim::Model(_entity);
|
||||
|
||||
if (!model.Valid(_ecm)) {
|
||||
RCLCPP_ERROR(logger,
|
||||
"[Vacuum Gripper Plugin] Failed to initialize because [%s] "
|
||||
"(Entity=%lu)] is not a model."
|
||||
"Please make sure that Vacuum Gripper Plugin is attached to a "
|
||||
"valid model.",
|
||||
model.Name(_ecm).c_str(), _entity);
|
||||
return;
|
||||
}
|
||||
|
||||
auto sdfPtr = const_cast<sdf::Element *>(_element.get());
|
||||
|
||||
if (!sdfPtr->HasElement("link_name")) {
|
||||
RCLCPP_ERROR(logger, "VacuumGripper link has not defined");
|
||||
return;
|
||||
}
|
||||
|
||||
this->dataPtr->gripper_link_name =
|
||||
sdfPtr->GetElement("link_name")->Get<std::string>();
|
||||
|
||||
_ecm.Each<gz::sim::components::Name>(
|
||||
[&](const gz::sim::Entity &entity,
|
||||
const gz::sim::components::Name *nameComp) -> bool {
|
||||
if (nameComp->Data() == this->dataPtr->gripper_link_name) {
|
||||
this->dataPtr->gripperEntity = entity;
|
||||
gzdbg << "Found gripper link: " << this->dataPtr->gripper_link_name
|
||||
<< " (Entity ID: " << entity << ")" << std::endl;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
});
|
||||
|
||||
if (this->dataPtr->gripperEntity == gz::sim::kNullEntity) {
|
||||
RCLCPP_ERROR(logger, "Gripper link '%s' not found!",
|
||||
this->dataPtr->gripper_link_name.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
std::string node_name = "gz_ros2_vacuum_gripper_plugin";
|
||||
|
||||
this->dataPtr->node_ = rclcpp::Node::make_shared(node_name);
|
||||
|
||||
this->dataPtr->executor_ =
|
||||
std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
|
||||
this->dataPtr->executor_->add_node(this->dataPtr->node_);
|
||||
auto spin = [this]() { this->dataPtr->executor_->spin(); };
|
||||
this->dataPtr->thread_executor_spin_ = std::thread(spin);
|
||||
|
||||
toggle_service_ =
|
||||
this->dataPtr->node_->create_service<std_srvs::srv::SetBool>(
|
||||
node_name + "/toggle",
|
||||
std::bind(&VacuumGripper::Toggle, this, std::placeholders::_1,
|
||||
std::placeholders::_2));
|
||||
|
||||
gzdbg << "rbs_mill_assist::VacuumGripper::Configure on entity: " << _entity
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
void VacuumGripper::PreUpdate(const gz::sim::UpdateInfo &_info,
|
||||
gz::sim::EntityComponentManager &_ecm) {
|
||||
if (!this->dataPtr->enabled) {
|
||||
if (this->dataPtr->contactedEntity != gz::sim::kNullEntity) {
|
||||
gzdbg << "Detaching object..." << std::endl;
|
||||
if (this->DetachLink(_ecm)) {
|
||||
gzdbg << "Object detached successfully!" << std::endl;
|
||||
} else {
|
||||
gzerr << "Failed to detach object!" << std::endl;
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
_ecm.Each<gz::sim::components::ContactSensorData>(
|
||||
[&](const gz::sim::Entity &entity,
|
||||
const gz::sim::components::ContactSensorData *contactData) -> bool {
|
||||
if (contactData->Data().contact_size() == 0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
auto msgEntity = contactData->Data().contact(0).collision2();
|
||||
gz::sim::Entity contactedCollision(msgEntity.id());
|
||||
|
||||
auto parentLink = _ecm.Component<gz::sim::components::ParentEntity>(
|
||||
contactedCollision);
|
||||
if (!parentLink) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (this->dataPtr->contactedEntity != gz::sim::kNullEntity) {
|
||||
return true;
|
||||
}
|
||||
|
||||
gz::sim::Entity gripperLink = this->dataPtr->gripperEntity;
|
||||
gz::sim::Entity objectLink = parentLink->Data();
|
||||
|
||||
gzdbg << "Attaching object " << objectLink << " to gripper..."
|
||||
<< std::endl;
|
||||
if (this->AttachLink(_ecm, gripperLink, objectLink)) {
|
||||
this->dataPtr->contactedEntity = objectLink;
|
||||
gzdbg << "Object attached successfully!" << std::endl;
|
||||
} else {
|
||||
gzerr << "Failed to attach object!" << std::endl;
|
||||
}
|
||||
|
||||
return true;
|
||||
});
|
||||
}
|
||||
|
||||
} // namespace rbs_mill_assist
|
||||
|
|
@ -17,43 +17,6 @@
|
|||
<xacro:include filename="$(find rbs_mill_assist)/urdf/rbs_arm_modular_macro.xacro" />
|
||||
<link name="world" />
|
||||
|
||||
<!-- World objects-->
|
||||
<xacro:property name="table_length" value="1.2"/>
|
||||
<xacro:property name="table_width" value="0.7"/>
|
||||
<xacro:property name="table_height" value="0.8"/>
|
||||
|
||||
<link name="table_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${table_length} ${table_width} ${table_height}"/>
|
||||
</geometry>
|
||||
<material name="blue">
|
||||
<color rgba="0 0 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${table_length} ${table_width} ${table_height}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10.0"/>
|
||||
<inertia
|
||||
ixx="${(1/12)*50*(table_width**2 + table_height**2)}"
|
||||
iyy="${(1/12)*50*(table_length**2 + table_height**2)}"
|
||||
izz="${(1/12)*50*(table_length**2 + table_width**2)}"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyz="0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="world_joint" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="table_link"/>
|
||||
<origin xyz="0 0 -0.4" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<xacro:property name="machine_length" value="0.4"/>
|
||||
<xacro:property name="machine_width" value="0.5"/>
|
||||
<xacro:property name="machine_height" value="0.115"/>
|
||||
|
|
|
|||
|
|
@ -47,7 +47,17 @@
|
|||
<namespace>${namespace}</namespace>
|
||||
</ros>
|
||||
</plugin>
|
||||
<plugin filename="VacuumGripper" name="rbs_mill_assist::VacuumGripper">
|
||||
<link_name>ee_link</link_name>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:if>
|
||||
<gazebo reference="ee_link">
|
||||
<sensor name='sensor_contact' type='contact'>
|
||||
<contact>
|
||||
<collision>ee_link_collision</collision>
|
||||
</contact>
|
||||
</sensor>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
|
|
|
|||
|
|
@ -6,13 +6,22 @@
|
|||
<grid>false</grid>
|
||||
</scene>
|
||||
|
||||
<physics name="1ms" type="ignored">
|
||||
<physics name='1ms' type='ignored'>
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<dart>
|
||||
<collision_detector>bullet</collision_detector>
|
||||
<!-- <solver> -->
|
||||
<!-- <solver_type>pgs</solver_type> -->
|
||||
<!-- </solver> -->
|
||||
</dart>
|
||||
</physics>
|
||||
<plugin
|
||||
filename="gz-sim-physics-system"
|
||||
name="gz::sim::systems::Physics">
|
||||
<engine>
|
||||
<filename>gz-physics-dartsim-plugin</filename>
|
||||
</engine>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="gz-sim-sensors-system"
|
||||
|
|
@ -27,6 +36,9 @@
|
|||
filename="gz-sim-scene-broadcaster-system"
|
||||
name="gz::sim::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-contact-system"
|
||||
name="gz::sim::systems::Contact">
|
||||
</plugin>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
|
|
@ -73,6 +85,52 @@
|
|||
<!-- </visual> -->
|
||||
<!-- </link> -->
|
||||
<!-- </model> -->
|
||||
<model name="table">
|
||||
<static>1</static>
|
||||
<link name='table_link'>
|
||||
<pose>0 0 -0.40000000000000002 0 0 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>10</mass>
|
||||
<inertia>
|
||||
<ixx>4.708333333333333</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>8.6666666666666661</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>8.0416666666666661</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='table_link_collision'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.2 0.69999999999999996 0.80000000000000004</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
<bounce/>
|
||||
<contact/>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='table_link_visual'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>1.2 0.69999999999999996 0.80000000000000004</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<material>
|
||||
<diffuse>0 0 1 1</diffuse>
|
||||
<ambient>0 0 1 1</ambient>
|
||||
</material>
|
||||
</visual>
|
||||
<enable_wind>false</enable_wind>
|
||||
</link>
|
||||
</model>
|
||||
|
||||
|
||||
</world>
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue