From cc4077670d4d3c2f92ba5653e8d9a7986cd2796c Mon Sep 17 00:00:00 2001 From: aquintan Date: Wed, 1 Jul 2026 10:27:44 +0200 Subject: [PATCH 1/7] Electromagnet plugin and add it to the robot for the amazon delivery exercise --- Industrial/drone_gripper/CMakeLists.txt | 70 +++++ .../drone_gripper/env-hooks/drone_gripper.dsv | 1 + Industrial/drone_gripper/package.xml | 25 ++ .../drone_gripper/src/drone_gripper.cpp | 268 ++++++++++++++++++ Launchers/package_delivery.launch.py | 11 + Worlds/package_delivery.world | 8 + 6 files changed, 383 insertions(+) create mode 100644 Industrial/drone_gripper/CMakeLists.txt create mode 100644 Industrial/drone_gripper/env-hooks/drone_gripper.dsv create mode 100644 Industrial/drone_gripper/package.xml create mode 100644 Industrial/drone_gripper/src/drone_gripper.cpp diff --git a/Industrial/drone_gripper/CMakeLists.txt b/Industrial/drone_gripper/CMakeLists.txt new file mode 100644 index 000000000..46afb539a --- /dev/null +++ b/Industrial/drone_gripper/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(drone_gripper) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +############################ +# Dependencies +############################ + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +find_package(gz-sim8 REQUIRED) +find_package(gz-plugin2 REQUIRED) + +############################ +# Plugin library +############################ + +add_library(drone_gripper SHARED + src/drone_gripper.cpp +) + +target_link_libraries(drone_gripper + gz-sim8::gz-sim8 + gz-plugin2::gz-plugin2 +) + +ament_target_dependencies(drone_gripper + rclcpp + std_msgs +) + +############################ +# Install plugin +############################ + +install( + TARGETS drone_gripper + LIBRARY DESTINATION lib +) + +############################ +# Environment hook (.dsv) +############################ + +install( + FILES env-hooks/drone_gripper.dsv + DESTINATION share/${PROJECT_NAME}/environment +) + +############################ +# Export +############################ + +ament_export_libraries(drone_gripper) + +ament_export_dependencies( + gz-sim8 + gz-plugin2 +) + +ament_package() diff --git a/Industrial/drone_gripper/env-hooks/drone_gripper.dsv b/Industrial/drone_gripper/env-hooks/drone_gripper.dsv new file mode 100644 index 000000000..a60f77078 --- /dev/null +++ b/Industrial/drone_gripper/env-hooks/drone_gripper.dsv @@ -0,0 +1 @@ +prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib diff --git a/Industrial/drone_gripper/package.xml b/Industrial/drone_gripper/package.xml new file mode 100644 index 000000000..7d4c251e5 --- /dev/null +++ b/Industrial/drone_gripper/package.xml @@ -0,0 +1,25 @@ + + + + drone_gripper + 0.0.1 + + Gazebo Harmonic magnetic gripper system plugin for drones + + dev + + Apache-2.0 + + ament_cmake + + rclcpp + + std_msgs + + gz-sim + + + ament_cmake + + + diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp new file mode 100644 index 000000000..9ee27847f --- /dev/null +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -0,0 +1,268 @@ +// Drone magnetic gripper system plugin for gz-sim (Harmonic). +// +// Behaves like an electromagnet: while energized it attaches the nearest +// graspable model within a configurable distance to the gripper link using a +// physical DetachableJoint, and releases it when de-energized. It is driven +// through ROS2 topics so the same interface works for Python and C++ user code. +// +// SDF parameters (all optional): +// model that carries the magnet (default: drone0) +// link the payload is attached to (default: base_link) +// comma separated model names to grab (default: "") +// max distance to grab a payload, in m (default: 1.0) +// std_msgs/Bool, energize/de-energize (default: /drone0/gripper/magnet) +// std_msgs/Bool, currently carrying? (default: /drone0/gripper/attached) + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gz; +using namespace sim; + +namespace drone_gripper +{ + +class DroneGripper : + public System, + public ISystemConfigure, + public ISystemPreUpdate +{ + +public: + +DroneGripper() = default; + +~DroneGripper() +{ + if (this->executor) + this->executor->cancel(); + + if (this->rosThread.joinable()) + this->rosThread.join(); +} + +void Configure( + const Entity &, + const std::shared_ptr &_sdf, + EntityComponentManager &, + EventManager &) override +{ + this->gripperModelName = _sdf->Get("gripper_model", "drone0").first; + this->gripperLinkName = _sdf->Get("gripper_link", "base_link").first; + this->attachDistance = _sdf->Get("attach_distance", 1.0).first; + + const std::string graspable = _sdf->Get("graspable_models", "").first; + std::stringstream ss(graspable); + std::string item; + while (std::getline(ss, item, ',')) + { + item.erase(std::remove_if(item.begin(), item.end(), ::isspace), item.end()); + if (!item.empty()) + this->graspableModels.push_back(item); + } + + const std::string magnetTopic = _sdf->Get("magnet_topic", "/drone0/gripper/magnet").first; + const std::string stateTopic = _sdf->Get("state_topic", "/drone0/gripper/attached").first; + + if (!rclcpp::ok()) + { + int argc = 0; + char **argv = nullptr; + rclcpp::init(argc, argv); + } + + this->node = std::make_shared("drone_gripper"); + this->executor = std::make_shared(); + this->executor->add_node(this->node); + + this->magnetSub = this->node->create_subscription( + magnetTopic, 10, + [this](const std_msgs::msg::Bool::SharedPtr msg) + { + std::lock_guard lock(this->mutex); + this->magnetEnabled = msg->data; + }); + + this->statePub = this->node->create_publisher(stateTopic, 10); + + this->rosThread = std::thread([this]() + { + while (rclcpp::ok()) + { + this->executor->spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + }); + + std::cout << "[DroneGripper] Configured: " << this->gripperModelName + << "::" << this->gripperLinkName + << " attach_distance=" << this->attachDistance << std::endl; +} + +void PreUpdate( + const UpdateInfo &_info, + EntityComponentManager &_ecm) override +{ + if (_info.paused) + return; + + bool enabled; + { + std::lock_guard lock(this->mutex); + enabled = this->magnetEnabled; + } + + if (enabled && this->activeJoint == kNullEntity) + this->TryAttach(_ecm); + else if (!enabled && this->activeJoint != kNullEntity) + this->Detach(_ecm); + + // Publish carrying state at ~10 Hz. + if (++this->publishCounter >= 25) + { + this->publishCounter = 0; + std_msgs::msg::Bool m; + m.data = (this->activeJoint != kNullEntity); + this->statePub->publish(m); + } +} + +private: + +Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) +{ + return _ecm.EntityByComponents(components::Name(modelName), components::Model()); +} + +Entity FindLink( + EntityComponentManager &_ecm, + const std::string &modelName, + const std::string &linkName) +{ + const Entity modelEntity = this->FindModel(_ecm, modelName); + if (modelEntity == kNullEntity) + return kNullEntity; + + Entity result{kNullEntity}; + _ecm.Each( + [&](const Entity &_entity, + const components::Name *_name, + const components::ParentEntity *_parent) + { + if (_name->Data() == linkName && _parent->Data() == modelEntity) + { + result = _entity; + return false; + } + return true; + }); + + return result; +} + +// Energized magnet: attach the closest graspable model within range. +void TryAttach(EntityComponentManager &_ecm) +{ + const Entity gripperLink = + this->FindLink(_ecm, this->gripperModelName, this->gripperLinkName); + if (gripperLink == kNullEntity) + return; + + const math::Pose3d gripperPose = worldPose(gripperLink, _ecm); + + Entity bestChild = kNullEntity; + double bestDist = this->attachDistance; + + for (const auto &name : this->graspableModels) + { + const Entity modelEntity = this->FindModel(_ecm, name); + if (modelEntity == kNullEntity) + continue; + + const Entity childLink = Model(modelEntity).CanonicalLink(_ecm); + if (childLink == kNullEntity) + continue; + + const math::Pose3d childPose = worldPose(childLink, _ecm); + const double dist = (gripperPose.Pos() - childPose.Pos()).Length(); + if (dist <= bestDist) + { + bestDist = dist; + bestChild = childLink; + } + } + + if (bestChild == kNullEntity) + return; + + const Entity jointEntity = _ecm.CreateEntity(); + components::DetachableJoint joint; + joint.Data().parentLink = gripperLink; + joint.Data().childLink = bestChild; + joint.Data().jointType = "fixed"; + _ecm.CreateComponent(jointEntity, joint); + + this->activeJoint = jointEntity; + std::cout << "[DroneGripper] Attached payload (dist=" << bestDist << ")" << std::endl; +} + +void Detach(EntityComponentManager &_ecm) +{ + if (this->activeJoint == kNullEntity) + return; + + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; + std::cout << "[DroneGripper] Released payload" << std::endl; +} + +private: + +rclcpp::Node::SharedPtr node; +rclcpp::executors::SingleThreadedExecutor::SharedPtr executor; +rclcpp::Subscription::SharedPtr magnetSub; +rclcpp::Publisher::SharedPtr statePub; +std::thread rosThread; + +std::string gripperModelName; +std::string gripperLinkName; +std::vector graspableModels; +double attachDistance{1.0}; + +std::mutex mutex; +bool magnetEnabled{false}; +Entity activeJoint{kNullEntity}; +int publishCounter{0}; + +}; + +} + +GZ_ADD_PLUGIN( + drone_gripper::DroneGripper, + gz::sim::System, + gz::sim::ISystemConfigure, + gz::sim::ISystemPreUpdate +) diff --git a/Launchers/package_delivery.launch.py b/Launchers/package_delivery.launch.py index b5d59b47f..efe0b5a19 100644 --- a/Launchers/package_delivery.launch.py +++ b/Launchers/package_delivery.launch.py @@ -35,7 +35,18 @@ def generate_launch_description(): output="screen", ) + # Make the drone_gripper system plugin discoverable by gz. + drone_gripper_path = "/home/ws/install/drone_gripper/lib" + set_gz_plugin_path = AppendEnvironmentVariable( + name="GZ_SIM_SYSTEM_PLUGIN_PATH", value=drone_gripper_path + ) + set_ld_library_path = AppendEnvironmentVariable( + name="LD_LIBRARY_PATH", value=drone_gripper_path + ) + ld = LaunchDescription() + ld.add_action(set_gz_plugin_path) + ld.add_action(set_ld_library_path) ld.add_action(gazebo_server) ld.add_action(world_entity_cmd) diff --git a/Worlds/package_delivery.world b/Worlds/package_delivery.world index 9432ac80d..f2af7c12b 100644 --- a/Worlds/package_delivery.world +++ b/Worlds/package_delivery.world @@ -14,6 +14,14 @@ ogre2 + + drone0 + base_link + package_box_01 + 1.0 + /drone0/gripper/magnet + /drone0/gripper/attached + From fc107209ab30f7a8096c0fd0b667e7f422a73173 Mon Sep 17 00:00:00 2001 From: aquintan Date: Wed, 1 Jul 2026 11:28:09 +0200 Subject: [PATCH 2/7] drone gripper update (visual for the gripper in xacro), make the gripper plugin aware of the sim reset --- .../quadrotor/launch/quadrotor.launch.py | 4 +++ .../models/quadrotor/quadrotor.urdf.xacro | 24 ++++++++++++++ .../drone_gripper/src/drone_gripper.cpp | 33 +++++++++++++++++-- Worlds/package_delivery.world | 2 +- database/universes.sql | 3 +- 5 files changed, 62 insertions(+), 4 deletions(-) diff --git a/CustomRobots/quadrotor/launch/quadrotor.launch.py b/CustomRobots/quadrotor/launch/quadrotor.launch.py index 503617b14..d9748d7af 100644 --- a/CustomRobots/quadrotor/launch/quadrotor.launch.py +++ b/CustomRobots/quadrotor/launch/quadrotor.launch.py @@ -24,6 +24,7 @@ def launch_setup(context): Y = LaunchConfiguration("Y") gz_sensor = LaunchConfiguration("sensor") gz_namespace = LaunchConfiguration("namespace") + gz_gripper = LaunchConfiguration("gripper") package_dir = get_package_share_directory("custom_robots") @@ -31,6 +32,7 @@ def launch_setup(context): sensor = gz_sensor.perform(context) namespace = gz_namespace.perform(context) + gripper = gz_gripper.perform(context) bridge_yaml = os.path.join(package_dir, "params", f"quadrotor_{sensor}.yaml") @@ -49,6 +51,7 @@ def launch_setup(context): mappings={ "camera": "true" if sensor == "camera" else "false", "namespace": namespace, + "gripper": gripper, }, ).toxml() @@ -166,6 +169,7 @@ def generate_launch_description(): declared_arguments.append(DeclareLaunchArgument("P", default_value="0")) declared_arguments.append(DeclareLaunchArgument("Y", default_value="0")) declared_arguments.append(DeclareLaunchArgument("sensor", default_value="camera")) + declared_arguments.append(DeclareLaunchArgument("gripper", default_value="false")) declared_arguments.append( DeclareLaunchArgument( "namespace", description="Namespace to use", default_value="drone0" diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index 3de702d7e..d80945355 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -3,6 +3,7 @@ xmlns:xacro="http://ros.org/wiki/xacro"> + @@ -15,4 +16,27 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 9ee27847f..f1deaa5ac 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -47,7 +47,8 @@ namespace drone_gripper class DroneGripper : public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemReset { public: @@ -125,6 +126,16 @@ void PreUpdate( const UpdateInfo &_info, EntityComponentManager &_ecm) override { + // If the gripper model disappears (e.g. the drone is removed on reset), + // drop the joint so it never dangles and wedges the server. Runs even + // while paused, because reset removes the model with the world paused. + if (this->activeJoint != kNullEntity && + this->FindModel(_ecm, this->gripperModelName) == kNullEntity) + { + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; + } + if (_info.paused) return; @@ -149,6 +160,23 @@ void PreUpdate( } } +// Called on world reset: drop any joint and clear state so nothing +// references the drone that reset removes and re-creates. +void Reset( + const UpdateInfo &, + EntityComponentManager &_ecm) override +{ + if (this->activeJoint != kNullEntity) + { + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; + } + + std::lock_guard lock(this->mutex); + this->magnetEnabled = false; + this->publishCounter = 0; +} + private: Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) @@ -264,5 +292,6 @@ GZ_ADD_PLUGIN( drone_gripper::DroneGripper, gz::sim::System, gz::sim::ISystemConfigure, - gz::sim::ISystemPreUpdate + gz::sim::ISystemPreUpdate, + gz::sim::ISystemReset ) diff --git a/Worlds/package_delivery.world b/Worlds/package_delivery.world index f2af7c12b..c734a018d 100644 --- a/Worlds/package_delivery.world +++ b/Worlds/package_delivery.world @@ -18,7 +18,7 @@ drone0 base_link package_box_01 - 1.0 + 0.3 /drone0/gripper/magnet /drone0/gripper/attached diff --git a/database/universes.sql b/database/universes.sql index bc99c32b8..e57e0ad56 100644 --- a/database/universes.sql +++ b/database/universes.sql @@ -159,7 +159,7 @@ COPY public.universes (id, name, world_id, robot_id) FROM stdin; 49 Monaco Circuit 49 2 55 Rover 4wd Warehouse 55 12 56 Pick And Place World 56 1 -57 Package delivery 57 11 +57 Package delivery 57 25 58 Warehouse 1 58 13 59 Warehouse 2 59 13 60 Warehouse 1 Ackermann 58 14 @@ -251,6 +251,7 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config) FROM stdin 22 Turtlebot 3 Medium Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=med 23 Turtlebot 3 High Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=high 24 Ur5 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py ur5_robotiq sensor:=camera +25 Quadrotor Gripper /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone0 sensor:=camera namespace:=drone0 gripper:=true \. -- From c7ba91751c4f321368028148fb0fdfb8365dc525 Mon Sep 17 00:00:00 2001 From: aquintan Date: Wed, 1 Jul 2026 11:46:56 +0200 Subject: [PATCH 3/7] avoid ventral camera oclusion with magnet --- .../models/quadrotor/quadrotor.urdf.xacro | 38 +++++++++++++------ 1 file changed, 26 insertions(+), 12 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index d80945355..06511a235 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -10,6 +10,26 @@ + + + + + + + + + + + + + + + + + + + @@ -17,24 +37,18 @@ - + - - + + - - - - - - - - + - + From f8f90a3d33c981d119125252ca691ea5d22bb77f Mon Sep 17 00:00:00 2001 From: aquintan Date: Wed, 1 Jul 2026 12:06:44 +0200 Subject: [PATCH 4/7] reduce attach distance of the electromagnet --- Worlds/package_delivery.world | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Worlds/package_delivery.world b/Worlds/package_delivery.world index c734a018d..2fd025340 100644 --- a/Worlds/package_delivery.world +++ b/Worlds/package_delivery.world @@ -18,7 +18,7 @@ drone0 base_link package_box_01 - 0.3 + 0.15 /drone0/gripper/magnet /drone0/gripper/attached From 27c37662937e54ee7a184ba13d903db2d889d33b Mon Sep 17 00:00:00 2001 From: aquintan Date: Wed, 1 Jul 2026 12:28:37 +0200 Subject: [PATCH 5/7] fix reset when grabbing item --- .../drone_gripper/src/drone_gripper.cpp | 30 +++++++++++++++---- 1 file changed, 25 insertions(+), 5 deletions(-) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index f1deaa5ac..7d13207a0 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -126,11 +126,12 @@ void PreUpdate( const UpdateInfo &_info, EntityComponentManager &_ecm) override { - // If the gripper model disappears (e.g. the drone is removed on reset), - // drop the joint so it never dangles and wedges the server. Runs even - // while paused, because reset removes the model with the world paused. - if (this->activeJoint != kNullEntity && - this->FindModel(_ecm, this->gripperModelName) == kNullEntity) + // Drop the joint if the gripper (drone) is gone OR is being removed this + // very cycle. Reset removes drone0 first; detaching in the SAME cycle as the + // removal keeps the DetachableJoint from outliving the drone links, which + // would wedge the physics server and stop the drone re-spawning. Runs even + // while paused, because reset happens with the world paused. + if (this->activeJoint != kNullEntity && this->GripperGoneOrRemoving(_ecm)) { _ecm.RequestRemoveEntity(this->activeJoint); this->activeJoint = kNullEntity; @@ -184,6 +185,25 @@ Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) return _ecm.EntityByComponents(components::Name(modelName), components::Model()); } +// True if the gripper model no longer exists, or is marked for removal in the +// current update cycle (EachRemoved reports entities that will be erased at the +// end of this cycle). +bool GripperGoneOrRemoving(EntityComponentManager &_ecm) +{ + if (this->FindModel(_ecm, this->gripperModelName) == kNullEntity) + return true; + + bool removing = false; + _ecm.EachRemoved( + [&](const Entity &, const components::Model *, const components::Name *_name) + { + if (_name->Data() == this->gripperModelName) + removing = true; + return true; + }); + return removing; +} + Entity FindLink( EntityComponentManager &_ecm, const std::string &modelName, From 1503b34404c37e802c4757099ef3854baa030115 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Fri, 3 Jul 2026 12:52:59 +0200 Subject: [PATCH 6/7] Move wrong urdf definitions --- .../models/quadrotor/quadrotor.urdf.xacro | 40 +------------------ .../quadrotor/quadrotor_common.urdf.xacro | 39 +++++++++++++++++- 2 files changed, 40 insertions(+), 39 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index 06511a235..04648e633 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -6,51 +6,15 @@ + - - - - - - - - - - - - - - - - - - - - + - - - - - - - - - - - - - - - - diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro index 913703ff3..af9763798 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro @@ -10,6 +10,26 @@ + + + + + + + + + + + + + + + + + + + @@ -42,7 +62,7 @@ - + @@ -91,6 +111,23 @@ + + + + + + + + + + + + + + + + From 6f32e1be4605dca452cdf89d573f7e5063a53884 Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 3 Jul 2026 13:16:12 +0200 Subject: [PATCH 7/7] make robot, world independent --- .../models/quadrotor/quadrotor.urdf.xacro | 13 ++ .../drone_gripper/src/drone_gripper.cpp | 139 +++++++++++------- Worlds/package_delivery.world | 8 - 3 files changed, 96 insertions(+), 64 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index 06511a235..2b8e7fd2c 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -52,5 +52,18 @@ + + + + + base_link + 0.15 + /${namespace}/gripper/magnet + /${namespace}/gripper/attached + /${namespace}/gripper/graspable + + diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 7d13207a0..77f104762 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -1,17 +1,22 @@ // Drone magnetic gripper system plugin for gz-sim (Harmonic). // -// Behaves like an electromagnet: while energized it attaches the nearest -// graspable model within a configurable distance to the gripper link using a -// physical DetachableJoint, and releases it when de-energized. It is driven -// through ROS2 topics so the same interface works for Python and C++ user code. +// This is a MODEL plugin: it is attached to the drone in its own SDF/URDF, so +// it knows which robot it belongs to (no hard-coded model name) and works with +// renamed models and several drones at once. It behaves like an electromagnet: +// while energized it attaches the nearest graspable model within a configurable +// distance to the gripper link with a physical DetachableJoint, and releases it +// when de-energized. It is driven through ROS2 topics, so the same interface +// works for Python and C++ user code. +// +// The graspable models are NOT baked into the robot: the exercise publishes +// them on the graspable topic, keeping the robot independent of any world. // // SDF parameters (all optional): -// model that carries the magnet (default: drone0) -// link the payload is attached to (default: base_link) -// comma separated model names to grab (default: "") -// max distance to grab a payload, in m (default: 1.0) -// std_msgs/Bool, energize/de-energize (default: /drone0/gripper/magnet) -// std_msgs/Bool, currently carrying? (default: /drone0/gripper/attached) +// link the payload is attached to (default: base_link) +// max distance to grab a payload, in m (default: 0.15) +// std_msgs/Bool, energize/de-energize (default: //gripper/magnet) +// std_msgs/Bool, currently carrying? (default: //gripper/attached) +// std_msgs/String, CSV grabbable names (default: //gripper/graspable) #include #include @@ -27,6 +32,7 @@ #include #include +#include #include #include @@ -65,27 +71,24 @@ DroneGripper() = default; } void Configure( - const Entity &, + const Entity &_entity, const std::shared_ptr &_sdf, - EntityComponentManager &, + EntityComponentManager &_ecm, EventManager &) override { - this->gripperModelName = _sdf->Get("gripper_model", "drone0").first; - this->gripperLinkName = _sdf->Get("gripper_link", "base_link").first; - this->attachDistance = _sdf->Get("attach_distance", 1.0).first; - - const std::string graspable = _sdf->Get("graspable_models", "").first; - std::stringstream ss(graspable); - std::string item; - while (std::getline(ss, item, ',')) - { - item.erase(std::remove_if(item.begin(), item.end(), ::isspace), item.end()); - if (!item.empty()) - this->graspableModels.push_back(item); - } + // Model plugin: _entity is the drone model that carries the magnet. + this->modelEntity = _entity; + + auto nameComp = _ecm.Component(_entity); + const std::string modelName = nameComp ? nameComp->Data() : "drone"; - const std::string magnetTopic = _sdf->Get("magnet_topic", "/drone0/gripper/magnet").first; - const std::string stateTopic = _sdf->Get("state_topic", "/drone0/gripper/attached").first; + this->gripperLinkName = _sdf->Get("gripper_link", "base_link").first; + this->attachDistance = _sdf->Get("attach_distance", 0.15).first; + + const std::string ns = "/" + modelName + "/gripper"; + const std::string magnetTopic = _sdf->Get("magnet_topic", ns + "/magnet").first; + const std::string stateTopic = _sdf->Get("state_topic", ns + "/attached").first; + const std::string graspableTopic = _sdf->Get("graspable_topic", ns + "/graspable").first; if (!rclcpp::ok()) { @@ -94,7 +97,7 @@ void Configure( rclcpp::init(argc, argv); } - this->node = std::make_shared("drone_gripper"); + this->node = std::make_shared("drone_gripper_" + modelName); this->executor = std::make_shared(); this->executor->add_node(this->node); @@ -106,6 +109,26 @@ void Configure( this->magnetEnabled = msg->data; }); + // The exercise defines what can be grabbed, so the robot stays independent + // of any world/object. Latched QoS so a list published once is not missed. + rclcpp::QoS graspableQos(10); + graspableQos.transient_local(); + this->graspableSub = this->node->create_subscription( + graspableTopic, graspableQos, + [this](const std_msgs::msg::String::SharedPtr msg) + { + std::lock_guard lock(this->mutex); + this->graspableModels.clear(); + std::stringstream ss(msg->data); + std::string item; + while (std::getline(ss, item, ',')) + { + item.erase(std::remove_if(item.begin(), item.end(), ::isspace), item.end()); + if (!item.empty()) + this->graspableModels.push_back(item); + } + }); + this->statePub = this->node->create_publisher(stateTopic, 10); this->rosThread = std::thread([this]() @@ -117,8 +140,8 @@ void Configure( } }); - std::cout << "[DroneGripper] Configured: " << this->gripperModelName - << "::" << this->gripperLinkName + std::cout << "[DroneGripper] Configured on model " << modelName + << " link=" << this->gripperLinkName << " attach_distance=" << this->attachDistance << std::endl; } @@ -126,11 +149,11 @@ void PreUpdate( const UpdateInfo &_info, EntityComponentManager &_ecm) override { - // Drop the joint if the gripper (drone) is gone OR is being removed this - // very cycle. Reset removes drone0 first; detaching in the SAME cycle as the - // removal keeps the DetachableJoint from outliving the drone links, which - // would wedge the physics server and stop the drone re-spawning. Runs even - // while paused, because reset happens with the world paused. + // Drop the joint if the drone is gone OR is being removed this very cycle. + // Reset removes the drone first; detaching in the SAME cycle as the removal + // keeps the DetachableJoint from outliving the drone links, which would wedge + // the physics server and stop the drone re-spawning. Runs even while paused, + // because reset happens with the world paused. if (this->activeJoint != kNullEntity && this->GripperGoneOrRemoving(_ecm)) { _ecm.RequestRemoveEntity(this->activeJoint); @@ -161,8 +184,8 @@ void PreUpdate( } } -// Called on world reset: drop any joint and clear state so nothing -// references the drone that reset removes and re-creates. +// Called on world reset: drop any joint and clear state so nothing references +// the drone that reset removes and re-creates. void Reset( const UpdateInfo &, EntityComponentManager &_ecm) override @@ -185,48 +208,43 @@ Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) return _ecm.EntityByComponents(components::Name(modelName), components::Model()); } -// True if the gripper model no longer exists, or is marked for removal in the -// current update cycle (EachRemoved reports entities that will be erased at the -// end of this cycle). +// True if our own drone model no longer exists, or is marked for removal in the +// current update cycle (EachRemoved reports entities erased at cycle end). bool GripperGoneOrRemoving(EntityComponentManager &_ecm) { - if (this->FindModel(_ecm, this->gripperModelName) == kNullEntity) + if (!_ecm.HasEntity(this->modelEntity)) return true; bool removing = false; - _ecm.EachRemoved( - [&](const Entity &, const components::Model *, const components::Name *_name) + _ecm.EachRemoved( + [&](const Entity &_e, const components::Model *) { - if (_name->Data() == this->gripperModelName) + if (_e == this->modelEntity) removing = true; return true; }); return removing; } -Entity FindLink( +// Find a link by name inside a given model. +Entity FindLinkInModel( EntityComponentManager &_ecm, - const std::string &modelName, + Entity model, const std::string &linkName) { - const Entity modelEntity = this->FindModel(_ecm, modelName); - if (modelEntity == kNullEntity) - return kNullEntity; - Entity result{kNullEntity}; _ecm.Each( [&](const Entity &_entity, const components::Name *_name, const components::ParentEntity *_parent) { - if (_name->Data() == linkName && _parent->Data() == modelEntity) + if (_name->Data() == linkName && _parent->Data() == model) { result = _entity; return false; } return true; }); - return result; } @@ -234,16 +252,24 @@ Entity FindLink( void TryAttach(EntityComponentManager &_ecm) { const Entity gripperLink = - this->FindLink(_ecm, this->gripperModelName, this->gripperLinkName); + this->FindLinkInModel(_ecm, this->modelEntity, this->gripperLinkName); if (gripperLink == kNullEntity) return; + std::vector graspables; + { + std::lock_guard lock(this->mutex); + graspables = this->graspableModels; + } + if (graspables.empty()) + return; + const math::Pose3d gripperPose = worldPose(gripperLink, _ecm); Entity bestChild = kNullEntity; double bestDist = this->attachDistance; - for (const auto &name : this->graspableModels) + for (const auto &name : graspables) { const Entity modelEntity = this->FindModel(_ecm, name); if (modelEntity == kNullEntity) @@ -291,13 +317,14 @@ void Detach(EntityComponentManager &_ecm) rclcpp::Node::SharedPtr node; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor; rclcpp::Subscription::SharedPtr magnetSub; +rclcpp::Subscription::SharedPtr graspableSub; rclcpp::Publisher::SharedPtr statePub; std::thread rosThread; -std::string gripperModelName; +Entity modelEntity{kNullEntity}; std::string gripperLinkName; std::vector graspableModels; -double attachDistance{1.0}; +double attachDistance{0.15}; std::mutex mutex; bool magnetEnabled{false}; diff --git a/Worlds/package_delivery.world b/Worlds/package_delivery.world index 2fd025340..9432ac80d 100644 --- a/Worlds/package_delivery.world +++ b/Worlds/package_delivery.world @@ -14,14 +14,6 @@ ogre2 - - drone0 - base_link - package_box_01 - 0.15 - /drone0/gripper/magnet - /drone0/gripper/attached -