From cc4077670d4d3c2f92ba5653e8d9a7986cd2796c Mon Sep 17 00:00:00 2001 From: aquintan Date: Wed, 1 Jul 2026 10:27:44 +0200 Subject: [PATCH 01/32] 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 02/32] 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 03/32] 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 04/32] 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 05/32] 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 06/32] 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 07/32] 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 - From 6be8d8a97840b3258f8794366be36e78ded8f099 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 6 Jul 2026 09:56:55 +0200 Subject: [PATCH 08/32] fix quadrator xacro --- .../models/quadrotor/quadrotor.urdf.xacro | 25 +++---------------- 1 file changed, 3 insertions(+), 22 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index 619cf183f..8a2518065 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -17,27 +17,10 @@ -<<<<<<< HEAD - - + - - - - - - - - - - - - - - base_link @@ -48,6 +31,4 @@ -======= ->>>>>>> upstream/drone-amazon-delivery From b3f91d21f942b43da53b26639edd50de470320c3 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 6 Jul 2026 10:20:35 +0200 Subject: [PATCH 09/32] electromagnet plugin in macro --- .../models/quadrotor/quadrotor.urdf.xacro | 14 -------------- .../models/quadrotor/quadrotor_common.urdf.xacro | 1 + .../models/quadrotor/quadrotor_gz.urdf.xacro | 14 ++++++++++++++ 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index 8a2518065..04648e633 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -17,18 +17,4 @@ - - - - - base_link - 0.15 - /${namespace}/gripper/magnet - /${namespace}/gripper/attached - /${namespace}/gripper/graspable - - - diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro index af9763798..643ba530d 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro @@ -126,6 +126,7 @@ + diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro index 400fd85fb..1fbf5edb3 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro @@ -45,6 +45,20 @@ + + + + + base_link + 0.15 + /${namespace}/gripper/magnet + /${namespace}/gripper/attached + /${namespace}/gripper/graspable + + + + From 691692a788c191c29908ba95089efca7749cbc78 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 6 Jul 2026 10:38:12 +0200 Subject: [PATCH 10/32] fix reset on drone gripper --- Industrial/drone_gripper/src/drone_gripper.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 77f104762..42cbb5c14 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,11 @@ DroneGripper() = default; ~DroneGripper() { + // Stop the ROS thread first. As a MODEL plugin this destructor runs every + // time the drone is removed (e.g. on reset), so it must return promptly; a + // hanging join would wedge gz and stop the drone re-spawning. + this->running_ = false; + if (this->executor) this->executor->cancel(); @@ -133,7 +139,7 @@ void Configure( this->rosThread = std::thread([this]() { - while (rclcpp::ok()) + while (this->running_ && rclcpp::ok()) { this->executor->spin_some(); std::this_thread::sleep_for(std::chrono::milliseconds(20)); @@ -320,6 +326,7 @@ rclcpp::Subscription::SharedPtr magnetSub; rclcpp::Subscription::SharedPtr graspableSub; rclcpp::Publisher::SharedPtr statePub; std::thread rosThread; +std::atomic running_{true}; Entity modelEntity{kNullEntity}; std::string gripperLinkName; From 77773277183fd79ea3f1f0f6f7c1aef73d7f601d Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 6 Jul 2026 10:58:14 +0200 Subject: [PATCH 11/32] fix drone reset when holding the package --- Industrial/drone_gripper/src/drone_gripper.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 42cbb5c14..c516589ca 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -167,7 +167,15 @@ void PreUpdate( } if (_info.paused) + { + // Reset pauses the world BEFORE removing the drone. Detach here so the box + // is never still jointed to the drone when it is removed (that leaves the + // box in a broken state and it never re-appears). It re-attaches on unpause + // via TryAttach below if the magnet is still energized. + if (this->activeJoint != kNullEntity) + this->Detach(_ecm); return; + } bool enabled; { From fbad6a01904eb901a6ea1e151a70adea4c5600c5 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 6 Jul 2026 11:49:22 +0200 Subject: [PATCH 12/32] fix magnet reset --- Industrial/drone_gripper/src/drone_gripper.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index c516589ca..47ce3d678 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -168,12 +168,18 @@ void PreUpdate( if (_info.paused) { - // Reset pauses the world BEFORE removing the drone. Detach here so the box - // is never still jointed to the drone when it is removed (that leaves the - // box in a broken state and it never re-appears). It re-attaches on unpause - // via TryAttach below if the magnet is still energized. + // A reset pauses the world before removing/resetting the drone. Detach the + // box (so it is not jointed to the drone when it is removed) AND de-energize + // the magnet. De-energizing is key: otherwise TryAttach would auto-re-attach + // on unpause, leaving the gripper stuck "carrying" a stale/reset box so it + // never grabs again. The exercise re-energizes the magnet when it wants to + // grab, so a fresh run works normally. if (this->activeJoint != kNullEntity) this->Detach(_ecm); + { + std::lock_guard lock(this->mutex); + this->magnetEnabled = false; + } return; } From 6024e6ba2a6795a59d513b9e48248f6c5889a268 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 6 Jul 2026 12:06:13 +0200 Subject: [PATCH 13/32] increase magnet pick distance --- CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro index 1fbf5edb3..4cddfa739 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro @@ -51,7 +51,7 @@ base_link - 0.15 + 0.2 /${namespace}/gripper/magnet /${namespace}/gripper/attached /${namespace}/gripper/graspable From 596d81f5c1a0fc64fe1704ff05a25e7a28b35f2e Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 6 Jul 2026 12:57:34 +0200 Subject: [PATCH 14/32] kill plugin node on reset --- .../drone_gripper/src/drone_gripper.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 47ce3d678..05ab4fdcf 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -144,6 +144,15 @@ void Configure( this->executor->spin_some(); std::this_thread::sleep_for(std::chrono::milliseconds(20)); } + // Tear down our ROS node from this thread (no race with spinning) so a + // plugin instance whose model was removed on reset does not linger as a + // zombie with a duplicate node name that corrupts the ROS graph. + if (this->executor && this->node) + this->executor->remove_node(this->node); + this->statePub.reset(); + this->magnetSub.reset(); + this->graspableSub.reset(); + this->node.reset(); }); std::cout << "[DroneGripper] Configured on model " << modelName @@ -155,6 +164,26 @@ void PreUpdate( const UpdateInfo &_info, EntityComponentManager &_ecm) override { + // Already shut down (our model was removed on reset): do nothing. + if (this->dead_) + return; + + // Our own model was removed (e.g. reset removed the drone). Release the box + // and shut ourselves down: stop the ROS thread so this instance stops being + // a zombie with a duplicate node name. gz recreates a fresh plugin for the + // re-spawned drone. + if (!_ecm.HasEntity(this->modelEntity)) + { + if (this->activeJoint != kNullEntity) + { + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; + } + this->running_ = false; + this->dead_ = true; + return; + } + // 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 @@ -341,6 +370,7 @@ rclcpp::Subscription::SharedPtr graspableSub; rclcpp::Publisher::SharedPtr statePub; std::thread rosThread; std::atomic running_{true}; +bool dead_{false}; Entity modelEntity{kNullEntity}; std::string gripperLinkName; From 472f1abfdd257fc4101d7682385d937694da78a0 Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 17 Jul 2026 12:02:06 +0200 Subject: [PATCH 15/32] fix gripper --- Industrial/drone_gripper/src/drone_gripper.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 05ab4fdcf..c6e66cbef 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -203,12 +203,25 @@ void PreUpdate( // on unpause, leaving the gripper stuck "carrying" a stale/reset box so it // never grabs again. The exercise re-energizes the magnet when it wants to // grab, so a fresh run works normally. + const bool wasCarrying = (this->activeJoint != kNullEntity); if (this->activeJoint != kNullEntity) this->Detach(_ecm); { std::lock_guard lock(this->mutex); this->magnetEnabled = false; } + // Announce the release immediately: this instance is about to be torn + // down (reset removes the drone next), so it never reaches the periodic + // publish below. Without this, HAL's cached "carrying" state stays stale + // (true) until the respawned drone's fresh plugin instance publishes its + // first heartbeat, which can make exercise code skip re-enabling the + // magnet right after a reset performed mid-carry. + if (wasCarrying) + { + std_msgs::msg::Bool m; + m.data = false; + this->statePub->publish(m); + } return; } From 4a5c2207a3c58c6cf2a9f269fd24b86de5445550 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Jul 2026 10:38:50 +0200 Subject: [PATCH 16/32] update package delivery world db --- database/worlds.sql | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/database/worlds.sql b/database/worlds.sql index 009328fe2..469598c83 100644 --- a/database/worlds.sql +++ b/database/worlds.sql @@ -337,7 +337,7 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config) FROM stdin 28 Dingo Low Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=low namespace:=do150 29 Dingo Medium Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=med namespace:=do150 30 Dingo High Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=high namespace:=do150 -31 Quadrotor Gripper /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone0 sensor:=camera namespace:=drone0 gripper:=true +31 Quadrotor Gripper /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone entity:=drone gripper:=true \. -- From e559bf50b869eb2a68c1cfdd2f88b9092da009c1 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 20 Jul 2026 12:54:45 +0200 Subject: [PATCH 17/32] fix magnet --- .../drone_gripper/src/drone_gripper.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 11383ed27..7accfde09 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -255,27 +255,24 @@ void Reset( // Detach because reset is tearing things down (drone removal or world pause), // as opposed to a normal exercise-triggered disable_magnet(). Announces the -// release immediately (the periodic heartbeat below never gets there, since -// this instance is about to die) and removes the payload model itself: a -// link that was ever the child of a DetachableJoint is left with corrupted -// physics-engine bookkeeping once that joint is removed, so a later -// DetachableJoint on the SAME link is created fine at the ECM level (state -// topic reports attached=true) but the constraint is never actually -// enforced, and the payload just sits on the ground. The WorldReset(all=true) -// the backend issues right after this recreates any entity missing relative -// to the world's initial snapshot, giving the payload a fresh physics body - -// the same way the drone itself is removed and respawned on every reset. +// release immediately: the periodic heartbeat below never gets there, since +// this instance is about to die, so without this HAL's cached "carrying" +// state stays stale (true) until the respawned drone's fresh plugin instance +// publishes its first heartbeat. +// +// NOTE: this used to also RequestRemoveEntity() the payload model itself, to +// work around a DetachableJoint that silently stops enforcing on a link that +// was jointed once before. That was reverted - removing a model entity +// during the paused/reset transition looks like the cause of gzserver dying +// after repeated carry-then-reset cycles. The stale-physics bug is still +// open; needs a safer fix. void HandleResetDetach(EntityComponentManager &_ecm) { - const Entity carriedModel = this->carriedModel; this->Detach(_ecm); std_msgs::msg::Bool m; m.data = false; this->statePub->publish(m); - - if (carriedModel != kNullEntity) - _ecm.RequestRemoveEntity(carriedModel); } Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) From ed153471cfae76765cc02a5a89f1380c124b3e1b Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:09:07 +0200 Subject: [PATCH 18/32] Rebase --- .../drone_gripper/src/drone_gripper.cpp | 648 +++++++++--------- Launchers/package_delivery.launch.py | 8 + Scenes/package_delivery.world | 8 + 3 files changed, 323 insertions(+), 341 deletions(-) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 7accfde09..a89afff18 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -12,412 +12,378 @@ // them on the graspable topic, keeping the robot independent of any world. // // SDF parameters (all optional): -// 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) +// 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 #include -#include -#include +#include #include +#include #include -#include -#include #include +#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 -{ +namespace drone_gripper { -class DroneGripper : - public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemReset -{ +class DroneGripper : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemReset { public: + DroneGripper() = default; -DroneGripper() = default; - -~DroneGripper() -{ - // Stop the ROS thread first. As a MODEL plugin this destructor runs every - // time the drone is removed (e.g. on reset), so it must return promptly; a - // hanging join would wedge gz and stop the drone re-spawning. - this->running_ = false; - - if (this->executor) - this->executor->cancel(); - - if (this->rosThread.joinable()) - this->rosThread.join(); -} - -void Configure( - const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &) override -{ - // 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"; - - 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()) - { - int argc = 0; - char **argv = nullptr; - rclcpp::init(argc, argv); - } + ~DroneGripper() { + // Stop the ROS thread first. As a MODEL plugin this destructor runs every + // time the drone is removed (e.g. on reset), so it must return promptly; a + // hanging join would wedge gz and stop the drone re-spawning. + this->running_ = false; - this->node = std::make_shared("drone_gripper_" + modelName); - this->executor = std::make_shared(); - this->executor->add_node(this->node); + if (this->executor) + this->executor->cancel(); - 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; - }); + if (this->rosThread.joinable()) + this->rosThread.join(); + } - // 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); + void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, EventManager &) override { + // 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"; + + 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()) { + int argc = 0; + char **argv = nullptr; + rclcpp::init(argc, argv); + } + + this->node = std::make_shared("drone_gripper_" + modelName); + 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; + }); + + // 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]() { + while (this->running_ && rclcpp::ok()) { + this->executor->spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); } + // Tear down our ROS node from this thread (no race with spinning) so a + // plugin instance whose model was removed on reset does not linger as a + // zombie with a duplicate node name that corrupts the ROS graph. + if (this->executor && this->node) + this->executor->remove_node(this->node); + this->statePub.reset(); + this->magnetSub.reset(); + this->graspableSub.reset(); + this->node.reset(); }); - this->statePub = this->node->create_publisher(stateTopic, 10); + std::cout << "[DroneGripper] Configured on model " << modelName + << " link=" << this->gripperLinkName + << " attach_distance=" << this->attachDistance << std::endl; + } - this->rosThread = std::thread([this]() - { - while (this->running_ && rclcpp::ok()) - { - this->executor->spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(20)); + void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) override { + // Already shut down (our model was removed on reset): do nothing. + if (this->dead_) + return; + + // Our own model was removed (e.g. reset removed the drone). Release the box + // and shut ourselves down: stop the ROS thread so this instance stops being + // a zombie with a duplicate node name. gz recreates a fresh plugin for the + // re-spawned drone. + if (!_ecm.HasEntity(this->modelEntity)) { + if (this->activeJoint != kNullEntity) { + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; + } + this->running_ = false; + this->dead_ = true; + return; } - // Tear down our ROS node from this thread (no race with spinning) so a - // plugin instance whose model was removed on reset does not linger as a - // zombie with a duplicate node name that corrupts the ROS graph. - if (this->executor && this->node) - this->executor->remove_node(this->node); - this->statePub.reset(); - this->magnetSub.reset(); - this->graspableSub.reset(); - this->node.reset(); - }); - - std::cout << "[DroneGripper] Configured on model " << modelName - << " link=" << this->gripperLinkName - << " attach_distance=" << this->attachDistance << std::endl; -} - -void PreUpdate( - const UpdateInfo &_info, - EntityComponentManager &_ecm) override -{ - // Already shut down (our model was removed on reset): do nothing. - if (this->dead_) - return; - - // Our own model was removed (e.g. reset removed the drone). Release the box - // and shut ourselves down: stop the ROS thread so this instance stops being - // a zombie with a duplicate node name. gz recreates a fresh plugin for the - // re-spawned drone. - if (!_ecm.HasEntity(this->modelEntity)) - { - if (this->activeJoint != kNullEntity) - { - _ecm.RequestRemoveEntity(this->activeJoint); - this->activeJoint = kNullEntity; + + // 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)) { + this->HandleResetDetach(_ecm); } - this->running_ = false; - this->dead_ = true; - return; - } - // 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)) - { - this->HandleResetDetach(_ecm); - } + if (_info.paused) { + // A reset pauses the world before removing/resetting the drone. Detach + // the box (so it is not jointed to the drone when it is removed) AND + // de-energize the magnet. De-energizing is key: otherwise TryAttach would + // auto-re-attach on unpause, leaving the gripper stuck "carrying" a + // stale/reset box so it never grabs again. The exercise re-energizes the + // magnet when it wants to grab, so a fresh run works normally. If + // GripperGoneOrRemoving already handled it above this cycle, activeJoint + // is already null here. + if (this->activeJoint != kNullEntity) + this->HandleResetDetach(_ecm); + { + std::lock_guard lock(this->mutex); + this->magnetEnabled = false; + } + return; + } - if (_info.paused) - { - // A reset pauses the world before removing/resetting the drone. Detach the - // box (so it is not jointed to the drone when it is removed) AND de-energize - // the magnet. De-energizing is key: otherwise TryAttach would auto-re-attach - // on unpause, leaving the gripper stuck "carrying" a stale/reset box so it - // never grabs again. The exercise re-energizes the magnet when it wants to - // grab, so a fresh run works normally. If GripperGoneOrRemoving already - // handled it above this cycle, activeJoint is already null here. - if (this->activeJoint != kNullEntity) - this->HandleResetDetach(_ecm); + bool enabled; { std::lock_guard lock(this->mutex); - this->magnetEnabled = false; + 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); } - return; } - bool enabled; - { + // 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; + } + this->carriedModel = kNullEntity; + std::lock_guard lock(this->mutex); - enabled = this->magnetEnabled; + this->magnetEnabled = false; + this->publishCounter = 0; } - if (enabled && this->activeJoint == kNullEntity) - this->TryAttach(_ecm); - else if (!enabled && this->activeJoint != kNullEntity) +private: + // Detach because reset is tearing things down (drone removal or world pause), + // as opposed to a normal exercise-triggered disable_magnet(). Announces the + // release immediately: the periodic heartbeat below never gets there, since + // this instance is about to die, so without this HAL's cached "carrying" + // state stays stale (true) until the respawned drone's fresh plugin instance + // publishes its first heartbeat. + // + // NOTE: this used to also RequestRemoveEntity() the payload model itself, to + // work around a DetachableJoint that silently stops enforcing on a link that + // was jointed once before. That was reverted - removing a model entity + // during the paused/reset transition looks like the cause of gzserver dying + // after repeated carry-then-reset cycles. The stale-physics bug is still + // open; needs a safer fix. + void HandleResetDetach(EntityComponentManager &_ecm) { 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); + m.data = false; this->statePub->publish(m); } -} - -// 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; - } - this->carriedModel = kNullEntity; - std::lock_guard lock(this->mutex); - this->magnetEnabled = false; - this->publishCounter = 0; -} - -private: - -// Detach because reset is tearing things down (drone removal or world pause), -// as opposed to a normal exercise-triggered disable_magnet(). Announces the -// release immediately: the periodic heartbeat below never gets there, since -// this instance is about to die, so without this HAL's cached "carrying" -// state stays stale (true) until the respawned drone's fresh plugin instance -// publishes its first heartbeat. -// -// NOTE: this used to also RequestRemoveEntity() the payload model itself, to -// work around a DetachableJoint that silently stops enforcing on a link that -// was jointed once before. That was reverted - removing a model entity -// during the paused/reset transition looks like the cause of gzserver dying -// after repeated carry-then-reset cycles. The stale-physics bug is still -// open; needs a safer fix. -void HandleResetDetach(EntityComponentManager &_ecm) -{ - this->Detach(_ecm); - - std_msgs::msg::Bool m; - m.data = false; - this->statePub->publish(m); -} - -Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) -{ - return _ecm.EntityByComponents(components::Name(modelName), components::Model()); -} - -// 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 (!_ecm.HasEntity(this->modelEntity)) - return true; - - bool removing = false; - _ecm.EachRemoved( - [&](const Entity &_e, const components::Model *) - { - if (_e == this->modelEntity) - removing = true; - return true; - }); - return removing; -} - -// Find a link by name inside a given model. -Entity FindLinkInModel( - EntityComponentManager &_ecm, - Entity model, - const std::string &linkName) -{ - Entity result{kNullEntity}; - _ecm.Each( - [&](const Entity &_entity, - const components::Name *_name, - const components::ParentEntity *_parent) - { - if (_name->Data() == linkName && _parent->Data() == model) - { - 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->FindLinkInModel(_ecm, this->modelEntity, this->gripperLinkName); - if (gripperLink == kNullEntity) - return; - - std::vector graspables; - { - std::lock_guard lock(this->mutex); - graspables = this->graspableModels; + Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) { + return _ecm.EntityByComponents(components::Name(modelName), + components::Model()); } - if (graspables.empty()) - return; - const math::Pose3d gripperPose = worldPose(gripperLink, _ecm); + // 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 (!_ecm.HasEntity(this->modelEntity)) + return true; - Entity bestChild = kNullEntity; - Entity bestModel = kNullEntity; - double bestDist = this->attachDistance; + bool removing = false; + _ecm.EachRemoved( + [&](const Entity &_e, const components::Model *) { + if (_e == this->modelEntity) + removing = true; + return true; + }); + return removing; + } - for (const auto &name : graspables) - { - const Entity candidateModel = this->FindModel(_ecm, name); - if (candidateModel == kNullEntity) - continue; + // Find a link by name inside a given model. + Entity FindLinkInModel(EntityComponentManager &_ecm, Entity model, + const std::string &linkName) { + Entity result{kNullEntity}; + _ecm.Each( + [&](const Entity &_entity, const components::Name *_name, + const components::ParentEntity *_parent) { + if (_name->Data() == linkName && _parent->Data() == model) { + result = _entity; + return false; + } + return true; + }); + return result; + } - const Entity childLink = Model(candidateModel).CanonicalLink(_ecm); - if (childLink == kNullEntity) - continue; + // Energized magnet: attach the closest graspable model within range. + void TryAttach(EntityComponentManager &_ecm) { + const Entity gripperLink = + this->FindLinkInModel(_ecm, this->modelEntity, this->gripperLinkName); + if (gripperLink == kNullEntity) + return; - const math::Pose3d childPose = worldPose(childLink, _ecm); - const double dist = (gripperPose.Pos() - childPose.Pos()).Length(); - if (dist <= bestDist) + std::vector graspables; { - bestDist = dist; - bestChild = childLink; - bestModel = candidateModel; + std::lock_guard lock(this->mutex); + graspables = this->graspableModels; + } + if (graspables.empty()) + return; + + const math::Pose3d gripperPose = worldPose(gripperLink, _ecm); + + Entity bestChild = kNullEntity; + Entity bestModel = kNullEntity; + double bestDist = this->attachDistance; + + for (const auto &name : graspables) { + const Entity candidateModel = this->FindModel(_ecm, name); + if (candidateModel == kNullEntity) + continue; + + const Entity childLink = Model(candidateModel).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; + bestModel = candidateModel; + } } - } - if (bestChild == kNullEntity) - return; + 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); + 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; - this->carriedModel = bestModel; - std::cout << "[DroneGripper] Attached payload (dist=" << bestDist << ")" << std::endl; -} + this->activeJoint = jointEntity; + this->carriedModel = bestModel; + std::cout << "[DroneGripper] Attached payload (dist=" << bestDist << ")" + << std::endl; + } -void Detach(EntityComponentManager &_ecm) -{ - if (this->activeJoint == kNullEntity) - return; + void Detach(EntityComponentManager &_ecm) { + if (this->activeJoint == kNullEntity) + return; - _ecm.RequestRemoveEntity(this->activeJoint); - this->activeJoint = kNullEntity; - this->carriedModel = kNullEntity; - std::cout << "[DroneGripper] Released payload" << std::endl; -} + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; + this->carriedModel = kNullEntity; + std::cout << "[DroneGripper] Released payload" << std::endl; + } private: - -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::atomic running_{true}; -bool dead_{false}; - -Entity modelEntity{kNullEntity}; -std::string gripperLinkName; -std::vector graspableModels; -double attachDistance{0.15}; - -std::mutex mutex; -bool magnetEnabled{false}; -Entity activeJoint{kNullEntity}; -Entity carriedModel{kNullEntity}; -int publishCounter{0}; - + 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::atomic running_{true}; + bool dead_{false}; + + Entity modelEntity{kNullEntity}; + std::string gripperLinkName; + std::vector graspableModels; + double attachDistance{0.15}; + + std::mutex mutex; + bool magnetEnabled{false}; + Entity activeJoint{kNullEntity}; + Entity carriedModel{kNullEntity}; + int publishCounter{0}; }; -} +} // namespace drone_gripper -GZ_ADD_PLUGIN( - drone_gripper::DroneGripper, - gz::sim::System, - gz::sim::ISystemConfigure, - gz::sim::ISystemPreUpdate, - gz::sim::ISystemReset -) +GZ_ADD_PLUGIN(drone_gripper::DroneGripper, gz::sim::System, + gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate, + gz::sim::ISystemReset) \ No newline at end of file diff --git a/Launchers/package_delivery.launch.py b/Launchers/package_delivery.launch.py index 24c9d2d7e..f5414ed0a 100644 --- a/Launchers/package_delivery.launch.py +++ b/Launchers/package_delivery.launch.py @@ -52,6 +52,14 @@ 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) diff --git a/Scenes/package_delivery.world b/Scenes/package_delivery.world index 9432ac80d..f2af7c12b 100644 --- a/Scenes/package_delivery.world +++ b/Scenes/package_delivery.world @@ -14,6 +14,14 @@ ogre2 + + drone0 + base_link + package_box_01 + 1.0 + /drone0/gripper/magnet + /drone0/gripper/attached + From 8e3e611437a5d5f93387ad432938925fcd783529 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:10:47 +0200 Subject: [PATCH 19/32] Rebase --- .../models/quadrotor/quadrotor.urdf.xacro | 24 +++++++++++++++++++ .../drone_gripper/src/drone_gripper.cpp | 2 +- Scenes/package_delivery.world | 2 +- 3 files changed, 26 insertions(+), 2 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index 0cc8a146b..a5b7f9623 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -5,6 +5,7 @@ + @@ -17,4 +18,27 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index a89afff18..b182a7668 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -386,4 +386,4 @@ class DroneGripper : public System, GZ_ADD_PLUGIN(drone_gripper::DroneGripper, gz::sim::System, gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate, - gz::sim::ISystemReset) \ No newline at end of file + gz::sim::ISystemReset) diff --git a/Scenes/package_delivery.world b/Scenes/package_delivery.world index f2af7c12b..c734a018d 100644 --- a/Scenes/package_delivery.world +++ b/Scenes/package_delivery.world @@ -18,7 +18,7 @@ drone0 base_link package_box_01 - 1.0 + 0.3 /drone0/gripper/magnet /drone0/gripper/attached From bd8ff86f26f8f1c5be20d6a73b838e6ffd68002a Mon Sep 17 00:00:00 2001 From: aquintan Date: Wed, 1 Jul 2026 12:06:44 +0200 Subject: [PATCH 20/32] reduce attach distance of the electromagnet --- Scenes/package_delivery.world | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Scenes/package_delivery.world b/Scenes/package_delivery.world index c734a018d..2fd025340 100644 --- a/Scenes/package_delivery.world +++ b/Scenes/package_delivery.world @@ -18,7 +18,7 @@ drone0 base_link package_box_01 - 0.3 + 0.15 /drone0/gripper/magnet /drone0/gripper/attached From e7bec74cb41e59ad337e8eabcae5978f657d7404 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:12:53 +0200 Subject: [PATCH 21/32] Rebase --- .../models/quadrotor/quadrotor.urdf.xacro | 13 ++ .../drone_gripper/src/drone_gripper.cpp | 119 ++++++++++++++++++ Scenes/package_delivery.world | 8 -- 3 files changed, 132 insertions(+), 8 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index a5b7f9623..d6241e8b1 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -40,5 +40,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 b182a7668..e2143c1d6 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -370,10 +370,129 @@ class DroneGripper : public System, std::atomic running_{true}; bool dead_{false}; +<<<<<<< HEAD Entity modelEntity{kNullEntity}; std::string gripperLinkName; std::vector graspableModels; double attachDistance{0.15}; +======= + Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) { + return _ecm.EntityByComponents(components::Name(modelName), + components::Model()); + } + + // 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 (!_ecm.HasEntity(this->modelEntity)) + return true; + + bool removing = false; + _ecm.EachRemoved( + [&](const Entity &_e, const components::Model *) { + if (_e == this->modelEntity) + removing = true; + return true; + }); + return removing; + } + + // Find a link by name inside a given model. + Entity FindLinkInModel(EntityComponentManager &_ecm, Entity model, + const std::string &linkName) { + Entity result{kNullEntity}; + _ecm.Each( + [&](const Entity &_entity, const components::Name *_name, + const components::ParentEntity *_parent) { + if (_name->Data() == linkName && _parent->Data() == model) { + 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->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 : graspables) { + 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::Subscription::SharedPtr graspableSub; + rclcpp::Publisher::SharedPtr statePub; + std::thread rosThread; + + Entity modelEntity{kNullEntity}; + std::string gripperLinkName; + std::vector graspableModels; + double attachDistance{0.15}; + + std::mutex mutex; + bool magnetEnabled{false}; + Entity activeJoint{kNullEntity}; + int publishCounter{0}; +>>>>>>> eb9540d6e (make robot, world independent) std::mutex mutex; bool magnetEnabled{false}; diff --git a/Scenes/package_delivery.world b/Scenes/package_delivery.world index 2fd025340..9432ac80d 100644 --- a/Scenes/package_delivery.world +++ b/Scenes/package_delivery.world @@ -14,14 +14,6 @@ ogre2 - - drone0 - base_link - package_box_01 - 0.15 - /drone0/gripper/magnet - /drone0/gripper/attached - From 79e820cd29f8fa68005f0318815cb9c5110c08fb Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:13:08 +0200 Subject: [PATCH 22/32] Rebase --- .../quadrotor/quadrotor_common.urdf.xacro | 192 +++++++++--------- 1 file changed, 98 insertions(+), 94 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro index d83f9aa8f..a98d6cc60 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro @@ -30,109 +30,113 @@ + <<<<<<< HEAD - - - - - +======= + +>>>>>>> 75f31203e (Rebase) + + + + + - - - - - - - - - - - - 1 - - + + + + + + + + + + + + 1 + + - - - - - - - - - + + + + + + + + + - + - - - - true - - - - - - - - - - - - - - - + + + + true + + + + + + + + + + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - - + + + + + + + - - - - + + + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - + + + + + + + From 01f80be30a35015b34474b489d5aae8e9c51f4dd Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:19:32 +0200 Subject: [PATCH 23/32] Rebase --- .../quadrotor/quadrotor_common.urdf.xacro | 194 +++++++++--------- 1 file changed, 95 insertions(+), 99 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro index a98d6cc60..c34e77531 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro @@ -30,113 +30,109 @@ - <<<<<<< HEAD - -======= - ->>>>>>> 75f31203e (Rebase) - - - - - + + + + + + - - - - - - - - - - - - 1 - - + + + + + + + + + + + + 1 + + - - - - - - - - - + + + + + + + + + - + - - - - true - - - - - - - - - - - - - - - + + + + true + + + + + + + + + + + + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - - + + + + + + + - - - - + + + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - + + + + + + + From 0168dbf104cf4ada70bbe88e5cf3a01e67001289 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:22:06 +0200 Subject: [PATCH 24/32] Rebase --- Industrial/drone_gripper/src/drone_gripper.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index e2143c1d6..cf30f5b88 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -371,10 +371,14 @@ class DroneGripper : public System, bool dead_{false}; <<<<<<< HEAD +<<<<<<< HEAD +======= +>>>>>>> e73d670a0 (Rebase) Entity modelEntity{kNullEntity}; std::string gripperLinkName; std::vector graspableModels; double attachDistance{0.15}; +<<<<<<< HEAD ======= Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) { return _ecm.EntityByComponents(components::Name(modelName), @@ -493,11 +497,16 @@ class DroneGripper : public System, Entity activeJoint{kNullEntity}; int publishCounter{0}; >>>>>>> eb9540d6e (make robot, world independent) +======= +>>>>>>> e73d670a0 (Rebase) std::mutex mutex; bool magnetEnabled{false}; Entity activeJoint{kNullEntity}; +<<<<<<< HEAD Entity carriedModel{kNullEntity}; +======= +>>>>>>> e73d670a0 (Rebase) int publishCounter{0}; }; From 3e0f3fcfc3dcf6fe88d5a99c68c1dc90cd1679f6 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:41:30 +0200 Subject: [PATCH 25/32] Bump db --- database/worlds.sql | 244 ++++++++++++++++++++++---------------------- 1 file changed, 120 insertions(+), 124 deletions(-) diff --git a/database/worlds.sql b/database/worlds.sql index 469598c83..ec0c4098d 100644 --- a/database/worlds.sql +++ b/database/worlds.sql @@ -63,7 +63,7 @@ CREATE TABLE public.worlds_robots ( id bigint NOT NULL, world_id bigint NOT NULL, robot_id bigint NOT NULL, - instances SMALLINT NOT NULL + poses real[] [6] NOT NULL ); ALTER TABLE public.worlds_robots OWNER TO "user-dev"; @@ -91,13 +91,9 @@ CREATE TABLE public.scenes ( tools_config character varying(200) NOT NULL, ros_version character varying(4) NOT NULL, type character varying(50) NOT NULL, - start_pose real[] [6] NOT NULL + model_path character varying(200) NOT NULL ); --- --- start_pose is '{X,Y,Z,Roll,Pitch,Yaw}' --- - ALTER TABLE public.scenes OWNER TO "user-dev"; -- @@ -121,7 +117,8 @@ CREATE TABLE public.robots ( name character varying(100) NOT NULL, launch_file_path character varying(200) NOT NULL, entity character varying(32) NOT NULL, - extra_config character varying(256) NOT NULL + extra_config character varying(256) NOT NULL, + model_path character varying(200) NOT NULL ); ALTER TABLE public.robots OWNER TO "user-dev"; @@ -209,54 +206,54 @@ COPY public.worlds (id, name, scene_id) FROM stdin; -- -COPY public.worlds_robots (id, world_id, robot_id, instances) FROM stdin; -0 1 9 1 -1 5 2 1 -2 6 2 1 -3 11 18 1 -4 12 16 1 -5 20 2 1 -6 21 2 1 -7 22 4 1 -8 23 4 1 -9 24 4 1 -10 25 4 1 -11 27 10 1 -12 29 21 1 -13 30 28 1 -14 31 11 1 -15 32 11 1 -16 33 28 1 -17 35 6 1 -18 36 11 1 -19 37 11 1 -20 39 11 1 -21 40 3 1 -22 41 8 1 -23 42 8 1 -24 43 8 1 -25 44 7 1 -26 47 2 1 -27 48 4 1 -28 49 2 1 -29 55 12 1 -30 56 1 1 -31 57 31 1 -32 58 13 1 -33 59 13 1 -34 60 14 1 -35 61 14 1 -36 62 29 1 -37 63 30 1 -38 64 29 1 -39 65 30 1 -40 66 15 1 -41 67 15 1 -42 68 19 1 -43 69 20 1 -44 70 24 1 -45 71 26 1 -46 72 11 2 +COPY public.worlds_robots (id, world_id, robot_id, poses) FROM stdin; +0 1 9 {{-1,1.5,0,0.0,0.0,0.0}} +1 5 2 {{53.462,-10.734,0.004,0,0,-1.57}} +2 6 2 {{-200.88, -90.72, 0.0, 0.0, 0.0, -2.83}} +3 11 18 {{0,0,0.1,0,0,-1.5529944}} +4 12 16 {{0.0,0.0,0.0,0.0,0.0,0.0}} +5 20 2 {{27.18, -31.55, 0.0, 0.0, 0.01, -3.12}} +6 21 2 {{-74.29, 37.74, 0.0, 0.0, 0.0, -0.51}} +7 22 4 {{-74.29, 37.74, 0.0, 0.0, 0.0, -0.51}} +8 23 4 {{27.18, -31.55, 0.0, 0.0, 0.01, -3.12}} +9 24 4 {{-200.88, -90.72, 0.0, 0.0, 0.0, -2.83}} +10 25 4 {{53.462,-10.734,0.004,0,0,-1.57}} +11 27 10 {{-1,1.5,0.0,0.0,0.0,0.0}} +12 29 21 {{1,-1.5,0.43,0,0,0}} +13 30 28 {{0.0,0.0,0.0,0.0,0.0,0.0}} +14 31 11 {{0.0,0.0,1.45,0.0,0.0,0.0}} +15 32 11 {{17.96,0.0,0.3,0,0,-2.0}} +16 33 28 {{14.25,-10.75,0.1,0,-0,3.14}} +17 35 6 {{2.5,-30,0.1,0,0,1.57}} +18 36 11 {{0.0,0.0,1.05,0.0,0.0,0.0}} +19 37 11 {{-21.0,-4.0,0.15,0,0,0}} +20 39 11 {{-18,-8.5,0.3,0,0,0}} +21 40 3 {{0.04,0.68,0,0,0,-1.57}} +22 41 8 {{-7,2.5,0.004,0.0,0.0,0}} +23 42 8 {{-7,2.5,0.004,0.0,0.0,0}} +24 43 8 {{-7,2.5,0.004,0.0,0.0,0}} +25 44 7 {{-7,2.5,0.004,0.0,0.0,0}} +26 47 2 {{146,60,-593.20,0,0,0.35}} +27 48 4 {{-105.223, -70.77, -1.8, 0.0, 0.0, 1.69}} +28 49 2 {{-105.223, -70.77, -1.8, 0.0, 0.0, 1.69}} +29 55 12 {{0.0,0.0,0.15,0.0,0.0,0.0}} +30 56 1 {{0.0,0.0,0.9,0.0,0.0,0.0}} +31 57 31 {{-1.0,-4.0,0.3,0,0,1.5729}} +32 58 13 {{0.0,0.0,0.1,0.0,0.0,0.0}} +33 59 13 {{0.0,0.0,0.1,0.0,0.0,0.0}} +34 60 14 {{0.0,0.0,0.1,0.0,0.0,0.0}} +35 61 14 {{0.0,0.0,0.1,0.0,0.0,0.0}} +36 62 29 {{14.25,-10.75,0.1,0,-0,3.14}} +37 63 30 {{14.25,-10.75,0.1,0,-0,3.14}} +38 64 29 {{0.0,0.0,0.0,0.0,0.0,0.0}} +39 65 30 {{0.0,0.0,0.0,0.0,0.0,0.0}} +40 66 15 {{-1.0,10.0,0.1,0.0,0.0,0.0}} +41 67 15 {{-1.0,10.0,0.1,0.0,0.0,0.0}} +42 68 19 {{0.0,0.0,0.0,0.0,0.0,0.0}} +43 69 20 {{0.0,0.0,0.0,0.0,0.0,0.0}} +44 70 24 {{0.0,0.0,0.9,0.0,0.0,0.0}} +45 71 26 {{-0.4,-0.5,0.8,0.0,0.0,0.0}} +46 72 11 {{0,5,0.2,0.0,0.0,0.0},{20,5,0.2,0.0,0.0,0.0}} \. -- @@ -264,41 +261,40 @@ COPY public.worlds_robots (id, world_id, robot_id, instances) FROM stdin; -- -COPY public.scenes (id, name, launch_file_path, tools_config, ros_version, type, start_pose) FROM stdin; -9 City Large /opt/jderobot/Launchers/basic_city.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/basic_city.config"} ROS2 gz {{0,0,0.1,0,0,-1.5529944}} -12 Laser Mapping Warehouse /opt/jderobot/Launchers/laser_mapping.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/laser_mapping.config"} ROS2 gz {{14.25,-10.75,0.1,0,-0,3.14}} -14 Montmelo Circuit /opt/jderobot/Launchers/montmelo_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/montmelo_circuit.config"} ROS2 gz {{27.18, -31.55, 0.0, 0.0, 0.01, -3.12}} -16 Montreal Circuit /opt/jderobot/Launchers/montreal_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/montreal_circuit.config"} ROS2 gz {{-200.88, -90.72, 0.0, 0.0, 0.0, -2.83}} -18 Nurburgring Circuit /opt/jderobot/Launchers/nurburgring_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/nurburgring_circuit.config"} ROS2 gz {{-74.29, 37.74, 0.0, 0.0, 0.0, -0.51}} -23 Simple Circuit /opt/jderobot/Launchers/simple_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/simple_circuit.config"} ROS2 gz {{53.462,-10.734,0.004,0,0,-1.57}} -24 Small House /opt/jderobot/Launchers/small_house.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/small_house.config"} ROS2 gz {{-1,1.5,0,0.0,0.0,0.0}} -25 Vacuums House Markers /opt/jderobot/Launchers/detailed_house.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/detailed_house.config"} ROS2 gz {{1,-1.5,0.43,0,0,0}} -26 Small House Roof /opt/jderobot/Launchers/small_house_roof.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/small_house_roof.config"} ROS2 gz {{-1,1.5,0.0,0.0,0.0,0.0}} -31 Rescue People /opt/jderobot/Launchers/rescue_people.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/rescue_people.config"} ROS2 gz {{0.0,0.0,1.45,0.0,0.0,0.0}} -32 Follow Road /opt/jderobot/Launchers/follow_road.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_road.config"} ROS2 gz {{17.96,0.0,0.3,0,0,-2.0}} -33 Small Laser Mapping Warehouse /opt/jderobot/Launchers/small_laser_mapping.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/small_laser_mapping.config"} ROS2 gz {{0.0,0.0,0.0,0.0,0.0,0.0}} -35 Car Junction /opt/jderobot/Launchers/car_junction.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/car_junction.config"} ROS2 gz {{2.5,-30,0.1,0,0,1.57}} -36 Drone Gymkhana /opt/jderobot/Launchers/drone_gymkhana.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/drone_gymkhana.config"} ROS2 gz {{0.0,0.0,1.05,0.0,0.0,0.0}} -37 Tower Inspection /opt/jderobot/Launchers/power_tower_inspection.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/power_tower_inspection.config"} ROS2 gz {{-21.0,-4.0,0.15,0,0,0}} -39 Labyrinth Escape /opt/jderobot/Launchers/labyrinth_escape.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/labyrinth_escape.config"} ROS2 gz {{-18,-8.5,0.3,0,0,0}} -40 Obstacle Avoidance /opt/jderobot/Launchers/obstacle_avoidance_h.launch.py None ROS2 gz {{0.04,0.68,0,0,0,-1.57}} -41 Autopark_line /opt/jderobot/Launchers/autopark_line.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz {{-7,2.5,0.004,0.0,0.0,0}} -42 Autopark_battery /opt/jderobot/Launchers/autopark_battery.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz {{-7,2.5,0.004,0.0,0.0,0}} -43 Autopark_sideways /opt/jderobot/Launchers/autopark_sideways.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz {{-7,2.5,0.004,0.0,0.0,0}} -46 3d Reconstruction /opt/jderobot/Launchers/3d_reconstruction.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/3d_reconstruction.config"} ROS2 gz {{0.0,0.0,0.0,0.0,0.0,0.0}} -47 Spa Circuit /opt/jderobot/Launchers/spa_circuit.launch.py None ROS2 gz {{146,60,-593.20,0,0,0.35}} -49 Monaco Circuit /opt/jderobot/Launchers/monaco_circuit.launch.py None ROS2 gz {{-105.223, -70.77, -1.8, 0.0, 0.0, 1.69}} -55 Rover 4wd Warehouse /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {{0.0,0.0,0.15,0.0,0.0,0.0}} -56 Pick And Place /opt/jderobot/Launchers/pick_place.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {{0.0,0.0,0.9,0.0,0.0,0.0}} -57 Package delivery /opt/jderobot/Launchers/package_delivery.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/package_delivery.config"} ROS2 gz {{-1.0,-4.0,0.3,0,0,1.5729}} -58 Warehouse 1 /opt/jderobot/Launchers/warehouse1.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/amazon_robot_harmonic.config"} ROS2 gz {{0.0,0.0,0.1,0.0,0.0,0.0}} -59 Warehouse 2 /opt/jderobot/Launchers/warehouse2.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/amazon_robot_harmonic.config"} ROS2 gz {{0.0,0.0,0.1,0.0,0.0,0.0}} -66 Follow Person /opt/jderobot/Launchers/follow_person_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {{-1.0,10.0,0.1,0.0,0.0,0.0}} -67 Follow Person Teleop /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz {{-1.0,10.0,0.1,0.0,0.0,0.0}} -68 Rover 4wd Warehouse Low Noise /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz {{0.0,0.0,0.0,0.0,0.0,0.0}} -70 Machine Vision /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz {{0.0,0.0,0.9,0.0,0.0,0.0}} -71 Conveyor /opt/jderobot/Launchers/sausage_exercise.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"} ROS2 gz {{-0.4,-0.5,0.8,0.0,0.0,0.0}} -72 Drone Cat Mouse /opt/jderobot/Launchers/drone_cat_mouse.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/drone_cat_mouse.config"} ROS2 gz {{0,5,0.2,0.0,0.0,0.0},{20,5,0.2,0.0,0.0,0.0}} +COPY public.scenes (id, name, launch_file_path, tools_config, ros_version, type, model_path) FROM stdin; +9 City Large /opt/jderobot/Launchers/basic_city.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/basic_city.config"} ROS2 gz basic_city.urdf +12 Laser Mapping Warehouse /opt/jderobot/Launchers/laser_mapping.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/laser_mapping.config"} ROS2 gz laser_map.urdf +14 Montmelo Circuit /opt/jderobot/Launchers/montmelo_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/montmelo_circuit.config"} ROS2 gz montmelo_line.urdf +16 Montreal Circuit /opt/jderobot/Launchers/montreal_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/montreal_circuit.config"} ROS2 gz montreal_line.urdf +18 Nurburgring Circuit /opt/jderobot/Launchers/nurburgring_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/nurburgring_circuit.config"} ROS2 gz nurburgring_line.urdf +23 Simple Circuit /opt/jderobot/Launchers/simple_circuit.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/simple_circuit.config"} ROS2 gz simple_circuit.urdf +24 Small House /opt/jderobot/Launchers/small_house.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/small_house.config"} ROS2 gz small_house.urdf +25 Vacuums House Markers /opt/jderobot/Launchers/detailed_house.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/detailed_house.config"} ROS2 gz detailed_house.urdf +26 Small House Roof /opt/jderobot/Launchers/small_house_roof.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/small_house_roof.config"} ROS2 gz small_house_roof.urdf +31 Rescue People /opt/jderobot/Launchers/rescue_people.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/rescue_people.config"} ROS2 gz rescue_people.urdf +32 Follow Road /opt/jderobot/Launchers/follow_road.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_road.config"} ROS2 gz follow_road.urdf +33 Small Laser Mapping Warehouse /opt/jderobot/Launchers/small_laser_mapping.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/small_laser_mapping.config"} ROS2 gz small_laser_mapping.urdf +35 Car Junction /opt/jderobot/Launchers/car_junction.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/car_junction.config"} ROS2 gz car_junction.urdf +36 Drone Gymkhana /opt/jderobot/Launchers/drone_gymkhana.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/drone_gymkhana.config"} ROS2 gz drone_gymkhana.urdf +37 Tower Inspection /opt/jderobot/Launchers/power_tower_inspection.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/power_tower_inspection.config"} ROS2 gz power_tower_inspection.urdf +39 Labyrinth Escape /opt/jderobot/Launchers/labyrinth_escape.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/labyrinth_escape.config"} ROS2 gz labyrinth_escape.urdf +40 Obstacle Avoidance /opt/jderobot/Launchers/obstacle_avoidance_h.launch.py None ROS2 gz simple_circuit_obstacles.urdf +41 Autopark_line /opt/jderobot/Launchers/autopark_line.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz autopark_line.urdf +42 Autopark_battery /opt/jderobot/Launchers/autopark_battery.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz autopark_battery.urdf +43 Autopark_sideways /opt/jderobot/Launchers/autopark_sideways.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/autoparking.config"} ROS2 gz autopark_sideways.urdf +46 3d Reconstruction /opt/jderobot/Launchers/3d_reconstruction.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/3d_reconstruction.config"} ROS2 gz kobuki_1_reconstruction3d.urdf +47 Spa Circuit /opt/jderobot/Launchers/spa_circuit.launch.py None ROS2 gz spa_circuit.urdf +49 Monaco Circuit /opt/jderobot/Launchers/monaco_circuit.launch.py None ROS2 gz monaco_circuit.urdf +55 Rover 4wd Warehouse /opt/jderobot/Launchers/rover_4wd_warehouse.launch.py None ROS2 gz rover4wd_warehouse.urdf +56 Pick And Place /opt/jderobot/Launchers/pick_place.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz pick_place.urdf +57 Package delivery /opt/jderobot/Launchers/package_delivery.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/package_delivery.config"} ROS2 gz package_delivery.urdf +58 Warehouse 1 /opt/jderobot/Launchers/warehouse1.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/amazon_robot_harmonic.config"} ROS2 gz warehouse1.urdf +59 Warehouse 2 /opt/jderobot/Launchers/warehouse2.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/amazon_robot_harmonic.config"} ROS2 gz warehouse2.urdf +66 Follow Person /opt/jderobot/Launchers/follow_person_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz hospital.urdf +67 Follow Person Teleop /opt/jderobot/Launchers/follow_person_teleop_harmonic.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/follow_person.config"} ROS2 gz hospital.urdf +70 Machine Vision /opt/jderobot/Launchers/machine_vision.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/pick_place_harmonic.launch.py"} ROS2 gz machine_vision.urdf +71 Conveyor /opt/jderobot/Launchers/sausage_exercise.launch.py {"rviz":"/opt/jderobot/Launchers/rviz/sausage_exercise.launch.py"} ROS2 gz sausage_exercise.urdf +72 Drone Cat Mouse /opt/jderobot/Launchers/drone_cat_mouse.launch.py {"gzsim":"/opt/jderobot/Launchers/visualization/drone_cat_mouse.config"} ROS2 gz drone_cat_mouse.urdf \. -- @@ -306,38 +302,38 @@ COPY public.scenes (id, name, launch_file_path, tools_config, ros_version, type, -- -COPY public.robots (id, name, launch_file_path, entity, extra_config) FROM stdin; -1 Ur5 /home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py ur5_robotiq None -2 F1 Holonomic Camera /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=holo sensor:=camera namespace:=f1 -3 F1 Holonomic Laser /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=holo sensor:=laser namespace:=f1 -4 F1 Ackermann Camera /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=ackermann sensor:=camera namespace:=f1 -5 F1 Ackermann Laser /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=ackermann sensor:=laser namespace:=f1 -6 Autonomous car Camera /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car sensor:=camera namespace:=autonomous_car -7 Autonomous car Lidar /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car sensor:=lidar namespace:=autonomous_car -8 Autonomous car 3 Lasers /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car sensor:=laser namespace:=autonomous_car -9 Vacuum cleaner Laser /home/ws/src/CustomRobots/vacuum_cleaner/launch/vacuum_cleaner.launch.py vacuum_cleaner sensor:=laser namespace:=vacuum_cleaner -10 Vacuum cleaner Camera /home/ws/src/CustomRobots/vacuum_cleaner/launch/vacuum_cleaner.launch.py vacuum_cleaner sensor:=camera namespace:=vacuum_cleaner -11 Quadrotor /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone -12 Rover 4wd /home/ws/src/CustomRobots/rover_4wd/launch/rover_4wd.launch.py rover_4wd namespace:=rover_4wd -13 Holonomic Logistic /home/ws/src/CustomRobots/logistic_holonomic_robot/launch/logistic_holonomic_robot.launch.py logistic_holonomic_robot namespace:=logistic_robot -14 Ackermann Logistic /home/ws/src/CustomRobots/logistic_ackermann_robot/launch/logistic_ackermann_robot.launch.py logistic_ackermann_robot namespace:=logistic_robot -15 TurtleBot 2 /home/ws/src/CustomRobots/Turtlebot2/launch/turtlebot2.launch.py turtlebot2 sensor:=camera namespace:=turtlebot2 -16 TurtleBot 2 Stereo /home/ws/src/CustomRobots/Turtlebot2/launch/turtlebot2.launch.py turtlebot2 sensor:=stereo namespace:=turtlebot2 -17 Turtlebot 3 /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 namespace:=turtlebot3 -18 Autonomous holonomic car /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car mode:=holonomic namespace:=autonomous_car -19 Rover 4wd Low Noise /home/ws/src/CustomRobots/rover_4wd/launch/rover_4wd.launch.py rover_4wd noise:=low namespace:=rover_4wd -20 Rover 4wd High Noise /home/ws/src/CustomRobots/rover_4wd/launch/rover_4wd.launch.py rover_4wd noise:=high namespace:=rover_4wd -21 Turtlebot 3 Low Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=low namespace:=turtlebot3 -22 Turtlebot 3 Medium Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=med namespace:=turtlebot3 -23 Turtlebot 3 High Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=high namespace:=turtlebot3 -24 Ur5 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py ur5_robotiq sensor:=camera -25 Ur3 /home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py ur3_robotiq None -26 Ur3 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py ur3_robotiq sensor:=camera -27 Dingo /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 namespace:=do150 -28 Dingo Low Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=low namespace:=do150 -29 Dingo Medium Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=med namespace:=do150 -30 Dingo High Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=high namespace:=do150 -31 Quadrotor Gripper /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone entity:=drone gripper:=true +COPY public.robots (id, name, launch_file_path, entity, extra_config, model_path) FROM stdin; +1 Ur5 /home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py ur5_robotiq None robot_arms/models/ur5/ur5.urdf.xacro +2 F1 Holonomic Camera /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=holo sensor:=camera namespace:=f1 f1/models/f1/f1.urdf.xacro +3 F1 Holonomic Laser /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=holo sensor:=laser namespace:=f1 f1/models/f1/f1.urdf.xacro +4 F1 Ackermann Camera /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=ackermann sensor:=camera namespace:=f1 f1/models/f1/f1.urdf.xacro +5 F1 Ackermann Laser /home/ws/src/CustomRobots/f1/launch/f1.launch.py f1 mode:=ackermann sensor:=laser namespace:=f1 f1/models/f1/f1.urdf.xacro +6 Autonomous car Camera /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car sensor:=camera namespace:=autonomous_car autonomous_car/models/autonomous_car/autonomous_car.urdf.xacro +7 Autonomous car Lidar /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car sensor:=lidar namespace:=autonomous_car autonomous_car/models/autonomous_car/autonomous_car.urdf.xacro +8 Autonomous car 3 Lasers /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car sensor:=laser namespace:=autonomous_car autonomous_car/models/autonomous_car/autonomous_car.urdf.xacro +9 Vacuum cleaner Laser /home/ws/src/CustomRobots/vacuum_cleaner/launch/vacuum_cleaner.launch.py vacuum_cleaner sensor:=laser namespace:=vacuum_cleaner vacuum_cleaner/models/vacuum_cleaner/vacuum_cleaner.urdf.xacro +10 Vacuum cleaner Camera /home/ws/src/CustomRobots/vacuum_cleaner/launch/vacuum_cleaner.launch.py vacuum_cleaner sensor:=camera namespace:=vacuum_cleaner vacuum_cleaner/models/vacuum_cleaner/vacuum_cleaner.urdf.xacro +11 Quadrotor /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone quadrotor/models/quadrotor/quadrotor.urdf.xacro +12 Rover 4wd /home/ws/src/CustomRobots/rover_4wd/launch/rover_4wd.launch.py rover_4wd namespace:=rover_4wd rover_4wd/model/rover_4wd/rover_4wd.urdf.xacro +13 Holonomic Logistic /home/ws/src/CustomRobots/logistic_holonomic_robot/launch/logistic_holonomic_robot.launch.py logistic_holonomic_robot namespace:=logistic_robot logistic_holonomic_robot/models/logistic_holonomic_robot/logistic_holonomic_robot.urdf.xacro +14 Ackermann Logistic /home/ws/src/CustomRobots/logistic_ackermann_robot/launch/logistic_ackermann_robot.launch.py logistic_ackermann_robot namespace:=logistic_robot logistic_ackermann_robot/models/logistic_ackermann_robot/logistic_ackermann_robot.urdf.xacro +15 TurtleBot 2 /home/ws/src/CustomRobots/Turtlebot2/launch/turtlebot2.launch.py turtlebot2 sensor:=camera namespace:=turtlebot2 Turtlebot2/model/turtlebot2/turtlebot2.urdf.xacro +16 TurtleBot 2 Stereo /home/ws/src/CustomRobots/Turtlebot2/launch/turtlebot2.launch.py turtlebot2 sensor:=stereo namespace:=turtlebot2 Turtlebot2/model/turtlebot2/turtlebot2.urdf.xacro +17 Turtlebot 3 /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 namespace:=turtlebot3 turtlebot3/models/turtlebot3/turtlebot3.urdf.xacro +18 Autonomous holonomic car /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py autonomous_car mode:=holonomic namespace:=autonomous_car autonomous_car/models/autonomous_car/autonomous_car.urdf.xacro +19 Rover 4wd Low Noise /home/ws/src/CustomRobots/rover_4wd/launch/rover_4wd.launch.py rover_4wd noise:=low namespace:=rover_4wd rover_4wd/model/rover_4wd/rover_4wd.urdf.xacro +20 Rover 4wd High Noise /home/ws/src/CustomRobots/rover_4wd/launch/rover_4wd.launch.py rover_4wd noise:=high namespace:=rover_4wd rover_4wd/model/rover_4wd/rover_4wd.urdf.xacro +21 Turtlebot 3 Low Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=low namespace:=turtlebot3 turtlebot3/models/turtlebot3/turtlebot3.urdf.xacro +22 Turtlebot 3 Medium Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=med namespace:=turtlebot3 turtlebot3/models/turtlebot3/turtlebot3.urdf.xacro +23 Turtlebot 3 High Noise /home/ws/src/CustomRobots/turtlebot3/launch/turtlebot3.launch.py turtlebot3 noise:=high namespace:=turtlebot3 turtlebot3/models/turtlebot3/turtlebot3.urdf.xacro +24 Ur5 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur5.launch.py ur5_robotiq sensor:=camera robot_arms/models/ur5/ur5.urdf.xacro +25 Ur3 /home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py ur3_robotiq None robot_arms/models/ur3/ur3.urdf.xacro +26 Ur3 Camera /home/ws/src/CustomRobots/robot_arms/launch/ur3.launch.py ur3_robotiq sensor:=camera robot_arms/models/ur3/ur3.urdf.xacro +27 Dingo /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 namespace:=do150 dingo/model/dingo/dingo.urdf.xacro +28 Dingo Low Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=low namespace:=do150 dingo/model/dingo/dingo.urdf.xacro +29 Dingo Medium Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=med namespace:=do150 dingo/model/dingo/dingo.urdf.xacro +30 Dingo High Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=high namespace:=do150 dingo/model/dingo/dingo.urdf.xacro +31 Quadrotor Magnet /home/ws/src/CustomRobots/dingquadrotoro/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone gripper:=true quadrotor/models/quadrotor/quadrotor.urdf.xacro \. -- @@ -411,8 +407,8 @@ CREATE INDEX exercises_world_name_459df99a_like ON public.worlds USING btree (na -- Name: worlds_robots worlds_robots_world_id_robot_id_155bee4e_uniq; Type: CONSTRAINT; Schema: public; Owner: user-dev -- -ALTER TABLE ONLY public.worlds_robots -ADD CONSTRAINT worlds_robots_world_id_robot_id_155bee4e_uniq UNIQUE (world_id, robot_id); +-- ALTER TABLE ONLY public.worlds_robots +-- ADD CONSTRAINT worlds_robots_world_id_robot_id_155bee4e_uniq UNIQUE (world_id, robot_id); -- -- Name: worlds_robots worlds_robots_pkey; Type: CONSTRAINT; Schema: public; Owner: user-dev From e61494027d3f87e3ede5b5c8226dff93b7b3d514 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 13:49:35 +0200 Subject: [PATCH 26/32] Fix quadrotor --- .../models/quadrotor/quadrotor.urdf.xacro | 36 ------------------- 1 file changed, 36 deletions(-) diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index d6241e8b1..55ca6e505 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -18,40 +18,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - base_link - 0.15 - /${namespace}/gripper/magnet - /${namespace}/gripper/attached - /${namespace}/gripper/graspable - - - From 615f095c9c54888979a83c24bc6c2868ff5e70b9 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 14:00:58 +0200 Subject: [PATCH 27/32] Fix plugin --- .../drone_gripper/src/drone_gripper.cpp | 764 ++++++++---------- 1 file changed, 335 insertions(+), 429 deletions(-) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index cf30f5b88..58bcebca6 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -12,506 +12,412 @@ // them on the graspable topic, keeping the robot independent of any world. // // SDF parameters (all optional): -// 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) +// 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 #include +#include -#include -#include #include +#include #include +#include -#include #include +#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 { +namespace drone_gripper +{ -class DroneGripper : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemReset { +class DroneGripper : + public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemReset +{ public: - DroneGripper() = default; - ~DroneGripper() { - // Stop the ROS thread first. As a MODEL plugin this destructor runs every - // time the drone is removed (e.g. on reset), so it must return promptly; a - // hanging join would wedge gz and stop the drone re-spawning. - this->running_ = false; - - if (this->executor) - this->executor->cancel(); - - if (this->rosThread.joinable()) - this->rosThread.join(); +DroneGripper() = default; + +~DroneGripper() +{ + // Stop the ROS thread first. As a MODEL plugin this destructor runs every + // time the drone is removed (e.g. on reset), so it must return promptly; a + // hanging join would wedge gz and stop the drone re-spawning. + this->running_ = false; + + if (this->executor) + this->executor->cancel(); + + if (this->rosThread.joinable()) + this->rosThread.join(); +} + +void Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &) override +{ + // 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"; + + 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()) + { + int argc = 0; + char **argv = nullptr; + rclcpp::init(argc, argv); } - void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, EventManager &) override { - // 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"; - - 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()) { - int argc = 0; - char **argv = nullptr; - rclcpp::init(argc, argv); - } + this->node = std::make_shared("drone_gripper_" + modelName); + this->executor = std::make_shared(); + this->executor->add_node(this->node); - this->node = std::make_shared("drone_gripper_" + modelName); - 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; - }); - - // 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]() { - while (this->running_ && rclcpp::ok()) { - this->executor->spin_some(); - std::this_thread::sleep_for(std::chrono::milliseconds(20)); - } - // Tear down our ROS node from this thread (no race with spinning) so a - // plugin instance whose model was removed on reset does not linger as a - // zombie with a duplicate node name that corrupts the ROS graph. - if (this->executor && this->node) - this->executor->remove_node(this->node); - this->statePub.reset(); - this->magnetSub.reset(); - this->graspableSub.reset(); - this->node.reset(); + 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; }); - std::cout << "[DroneGripper] Configured on model " << modelName - << " link=" << this->gripperLinkName - << " attach_distance=" << this->attachDistance << std::endl; - } - - void PreUpdate(const UpdateInfo &_info, - EntityComponentManager &_ecm) override { - // Already shut down (our model was removed on reset): do nothing. - if (this->dead_) - return; - - // Our own model was removed (e.g. reset removed the drone). Release the box - // and shut ourselves down: stop the ROS thread so this instance stops being - // a zombie with a duplicate node name. gz recreates a fresh plugin for the - // re-spawned drone. - if (!_ecm.HasEntity(this->modelEntity)) { - if (this->activeJoint != kNullEntity) { - _ecm.RequestRemoveEntity(this->activeJoint); - this->activeJoint = kNullEntity; + // 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->running_ = false; - this->dead_ = true; - return; - } + }); - // 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)) { - this->HandleResetDetach(_ecm); - } + this->statePub = this->node->create_publisher(stateTopic, 10); - if (_info.paused) { - // A reset pauses the world before removing/resetting the drone. Detach - // the box (so it is not jointed to the drone when it is removed) AND - // de-energize the magnet. De-energizing is key: otherwise TryAttach would - // auto-re-attach on unpause, leaving the gripper stuck "carrying" a - // stale/reset box so it never grabs again. The exercise re-energizes the - // magnet when it wants to grab, so a fresh run works normally. If - // GripperGoneOrRemoving already handled it above this cycle, activeJoint - // is already null here. - if (this->activeJoint != kNullEntity) - this->HandleResetDetach(_ecm); - { - std::lock_guard lock(this->mutex); - this->magnetEnabled = false; - } - return; + this->rosThread = std::thread([this]() + { + while (this->running_ && rclcpp::ok()) + { + this->executor->spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); } - - bool enabled; + // Tear down our ROS node from this thread (no race with spinning) so a + // plugin instance whose model was removed on reset does not linger as a + // zombie with a duplicate node name that corrupts the ROS graph. + if (this->executor && this->node) + this->executor->remove_node(this->node); + this->statePub.reset(); + this->magnetSub.reset(); + this->graspableSub.reset(); + this->node.reset(); + }); + + std::cout << "[DroneGripper] Configured on model " << modelName + << " link=" << this->gripperLinkName + << " attach_distance=" << this->attachDistance << std::endl; +} + +void PreUpdate( + const UpdateInfo &_info, + EntityComponentManager &_ecm) override +{ + // Already shut down (our model was removed on reset): do nothing. + if (this->dead_) + return; + + // Our own model was removed (e.g. reset removed the drone). Release the box + // and shut ourselves down: stop the ROS thread so this instance stops being + // a zombie with a duplicate node name. gz recreates a fresh plugin for the + // re-spawned drone. + if (!_ecm.HasEntity(this->modelEntity)) + { + if (this->activeJoint != kNullEntity) { - std::lock_guard lock(this->mutex); - enabled = this->magnetEnabled; + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; } + this->running_ = false; + this->dead_ = true; + return; + } - 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); - } + // 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)) + { + this->HandleResetDetach(_ecm); } - // 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; + if (_info.paused) + { + // A reset pauses the world before removing/resetting the drone. Detach the + // box (so it is not jointed to the drone when it is removed) AND de-energize + // the magnet. De-energizing is key: otherwise TryAttach would auto-re-attach + // on unpause, leaving the gripper stuck "carrying" a stale/reset box so it + // never grabs again. The exercise re-energizes the magnet when it wants to + // grab, so a fresh run works normally. If GripperGoneOrRemoving already + // handled it above this cycle, activeJoint is already null here. + if (this->activeJoint != kNullEntity) + this->HandleResetDetach(_ecm); + { + std::lock_guard lock(this->mutex); + this->magnetEnabled = false; } - this->carriedModel = kNullEntity; + return; + } + bool enabled; + { std::lock_guard lock(this->mutex); - this->magnetEnabled = false; - this->publishCounter = 0; + enabled = this->magnetEnabled; } -private: - // Detach because reset is tearing things down (drone removal or world pause), - // as opposed to a normal exercise-triggered disable_magnet(). Announces the - // release immediately: the periodic heartbeat below never gets there, since - // this instance is about to die, so without this HAL's cached "carrying" - // state stays stale (true) until the respawned drone's fresh plugin instance - // publishes its first heartbeat. - // - // NOTE: this used to also RequestRemoveEntity() the payload model itself, to - // work around a DetachableJoint that silently stops enforcing on a link that - // was jointed once before. That was reverted - removing a model entity - // during the paused/reset transition looks like the cause of gzserver dying - // after repeated carry-then-reset cycles. The stale-physics bug is still - // open; needs a safer fix. - void HandleResetDetach(EntityComponentManager &_ecm) { + 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 = false; + m.data = (this->activeJoint != kNullEntity); this->statePub->publish(m); } - - Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) { - return _ecm.EntityByComponents(components::Name(modelName), - components::Model()); - } - - // 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 (!_ecm.HasEntity(this->modelEntity)) - return true; - - bool removing = false; - _ecm.EachRemoved( - [&](const Entity &_e, const components::Model *) { - if (_e == this->modelEntity) - removing = true; - return true; - }); - return removing; +} + +// 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; } + this->carriedModel = kNullEntity; - // Find a link by name inside a given model. - Entity FindLinkInModel(EntityComponentManager &_ecm, Entity model, - const std::string &linkName) { - Entity result{kNullEntity}; - _ecm.Each( - [&](const Entity &_entity, const components::Name *_name, - const components::ParentEntity *_parent) { - if (_name->Data() == linkName && _parent->Data() == model) { - result = _entity; - return false; - } - return true; - }); - return result; - } + std::lock_guard lock(this->mutex); + this->magnetEnabled = false; + this->publishCounter = 0; +} - // Energized magnet: attach the closest graspable model within range. - void TryAttach(EntityComponentManager &_ecm) { - const Entity gripperLink = - this->FindLinkInModel(_ecm, this->modelEntity, this->gripperLinkName); - if (gripperLink == kNullEntity) - return; +private: - std::vector graspables; +// Detach because reset is tearing things down (drone removal or world pause), +// as opposed to a normal exercise-triggered disable_magnet(). Announces the +// release immediately: the periodic heartbeat below never gets there, since +// this instance is about to die, so without this HAL's cached "carrying" +// state stays stale (true) until the respawned drone's fresh plugin instance +// publishes its first heartbeat. +// +// NOTE: this used to also RequestRemoveEntity() the payload model itself, to +// work around a DetachableJoint that silently stops enforcing on a link that +// was jointed once before. That was reverted - removing a model entity +// during the paused/reset transition looks like the cause of gzserver dying +// after repeated carry-then-reset cycles. The stale-physics bug is still +// open; needs a safer fix. +void HandleResetDetach(EntityComponentManager &_ecm) +{ + this->Detach(_ecm); + + std_msgs::msg::Bool m; + m.data = false; + this->statePub->publish(m); +} + +Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) +{ + return _ecm.EntityByComponents(components::Name(modelName), components::Model()); +} + +// 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 (!_ecm.HasEntity(this->modelEntity)) + return true; + + bool removing = false; + _ecm.EachRemoved( + [&](const Entity &_e, const components::Model *) { - std::lock_guard lock(this->mutex); - graspables = this->graspableModels; - } - if (graspables.empty()) - return; - - const math::Pose3d gripperPose = worldPose(gripperLink, _ecm); - - Entity bestChild = kNullEntity; - Entity bestModel = kNullEntity; - double bestDist = this->attachDistance; - - for (const auto &name : graspables) { - const Entity candidateModel = this->FindModel(_ecm, name); - if (candidateModel == kNullEntity) - continue; - - const Entity childLink = Model(candidateModel).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; - bestModel = candidateModel; + if (_e == this->modelEntity) + removing = true; + return true; + }); + return removing; +} + +// Find a link by name inside a given model. +Entity FindLinkInModel( + EntityComponentManager &_ecm, + Entity model, + const std::string &linkName) +{ + Entity result{kNullEntity}; + _ecm.Each( + [&](const Entity &_entity, + const components::Name *_name, + const components::ParentEntity *_parent) + { + if (_name->Data() == linkName && _parent->Data() == model) + { + result = _entity; + return false; } - } - - 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; - this->carriedModel = bestModel; - 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; - this->carriedModel = kNullEntity; - std::cout << "[DroneGripper] Released payload" << std::endl; - } - -private: - 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::atomic running_{true}; - bool dead_{false}; - -<<<<<<< HEAD -<<<<<<< HEAD -======= ->>>>>>> e73d670a0 (Rebase) - Entity modelEntity{kNullEntity}; - std::string gripperLinkName; - std::vector graspableModels; - double attachDistance{0.15}; -<<<<<<< HEAD -======= - Entity FindModel(EntityComponentManager &_ecm, const std::string &modelName) { - return _ecm.EntityByComponents(components::Name(modelName), - components::Model()); + return true; + }); + return result; +} + +// Energized magnet: attach the closest graspable model within range. +void TryAttach(EntityComponentManager &_ecm) +{ + const Entity gripperLink = + 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; - // 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 (!_ecm.HasEntity(this->modelEntity)) - return true; + const math::Pose3d gripperPose = worldPose(gripperLink, _ecm); - bool removing = false; - _ecm.EachRemoved( - [&](const Entity &_e, const components::Model *) { - if (_e == this->modelEntity) - removing = true; - return true; - }); - return removing; - } + Entity bestChild = kNullEntity; + Entity bestModel = kNullEntity; + double bestDist = this->attachDistance; - // Find a link by name inside a given model. - Entity FindLinkInModel(EntityComponentManager &_ecm, Entity model, - const std::string &linkName) { - Entity result{kNullEntity}; - _ecm.Each( - [&](const Entity &_entity, const components::Name *_name, - const components::ParentEntity *_parent) { - if (_name->Data() == linkName && _parent->Data() == model) { - result = _entity; - return false; - } - return true; - }); - return result; - } + for (const auto &name : graspables) + { + const Entity candidateModel = this->FindModel(_ecm, name); + if (candidateModel == kNullEntity) + continue; - // Energized magnet: attach the closest graspable model within range. - void TryAttach(EntityComponentManager &_ecm) { - const Entity gripperLink = - this->FindLinkInModel(_ecm, this->modelEntity, this->gripperLinkName); - if (gripperLink == kNullEntity) - return; + const Entity childLink = Model(candidateModel).CanonicalLink(_ecm); + if (childLink == kNullEntity) + continue; - std::vector graspables; + const math::Pose3d childPose = worldPose(childLink, _ecm); + const double dist = (gripperPose.Pos() - childPose.Pos()).Length(); + if (dist <= bestDist) { - std::lock_guard lock(this->mutex); - graspables = this->graspableModels; + bestDist = dist; + bestChild = childLink; + bestModel = candidateModel; } - if (graspables.empty()) - return; - - const math::Pose3d gripperPose = worldPose(gripperLink, _ecm); - - Entity bestChild = kNullEntity; - double bestDist = this->attachDistance; - - for (const auto &name : graspables) { - const Entity modelEntity = this->FindModel(_ecm, name); - if (modelEntity == kNullEntity) - continue; + } - const Entity childLink = Model(modelEntity).CanonicalLink(_ecm); - if (childLink == kNullEntity) - continue; + if (bestChild == kNullEntity) + return; - const math::Pose3d childPose = worldPose(childLink, _ecm); - const double dist = (gripperPose.Pos() - childPose.Pos()).Length(); - if (dist <= bestDist) { - bestDist = dist; - bestChild = childLink; - } - } + const Entity jointEntity = _ecm.CreateEntity(); + components::DetachableJoint joint; + joint.Data().parentLink = gripperLink; + joint.Data().childLink = bestChild; + joint.Data().jointType = "fixed"; + _ecm.CreateComponent(jointEntity, joint); - if (bestChild == kNullEntity) - return; + this->activeJoint = jointEntity; + this->carriedModel = bestModel; + std::cout << "[DroneGripper] Attached payload (dist=" << bestDist << ")" << std::endl; +} - const Entity jointEntity = _ecm.CreateEntity(); - components::DetachableJoint joint; - joint.Data().parentLink = gripperLink; - joint.Data().childLink = bestChild; - joint.Data().jointType = "fixed"; - _ecm.CreateComponent(jointEntity, joint); +void Detach(EntityComponentManager &_ecm) +{ + if (this->activeJoint == kNullEntity) + return; - this->activeJoint = jointEntity; - std::cout << "[DroneGripper] Attached payload (dist=" << bestDist << ")" - << std::endl; - } + _ecm.RequestRemoveEntity(this->activeJoint); + this->activeJoint = kNullEntity; + this->carriedModel = kNullEntity; + std::cout << "[DroneGripper] Released payload" << std::endl; +} - void Detach(EntityComponentManager &_ecm) { - if (this->activeJoint == kNullEntity) - return; +private: - _ecm.RequestRemoveEntity(this->activeJoint); - this->activeJoint = kNullEntity; - std::cout << "[DroneGripper] Released payload" << std::endl; - } +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::atomic running_{true}; +bool dead_{false}; + +Entity modelEntity{kNullEntity}; +std::string gripperLinkName; +std::vector graspableModels; +double attachDistance{0.15}; + +std::mutex mutex; +bool magnetEnabled{false}; +Entity activeJoint{kNullEntity}; +Entity carriedModel{kNullEntity}; +int publishCounter{0}; -private: - rclcpp::Node::SharedPtr node; - rclcpp::executors::SingleThreadedExecutor::SharedPtr executor; - rclcpp::Subscription::SharedPtr magnetSub; - rclcpp::Subscription::SharedPtr graspableSub; - rclcpp::Publisher::SharedPtr statePub; - std::thread rosThread; - - Entity modelEntity{kNullEntity}; - std::string gripperLinkName; - std::vector graspableModels; - double attachDistance{0.15}; - - std::mutex mutex; - bool magnetEnabled{false}; - Entity activeJoint{kNullEntity}; - int publishCounter{0}; ->>>>>>> eb9540d6e (make robot, world independent) -======= ->>>>>>> e73d670a0 (Rebase) - - std::mutex mutex; - bool magnetEnabled{false}; - Entity activeJoint{kNullEntity}; -<<<<<<< HEAD - Entity carriedModel{kNullEntity}; -======= ->>>>>>> e73d670a0 (Rebase) - int publishCounter{0}; }; -} // namespace drone_gripper +} -GZ_ADD_PLUGIN(drone_gripper::DroneGripper, gz::sim::System, - gz::sim::ISystemConfigure, gz::sim::ISystemPreUpdate, - gz::sim::ISystemReset) +GZ_ADD_PLUGIN( + drone_gripper::DroneGripper, + gz::sim::System, + gz::sim::ISystemConfigure, + gz::sim::ISystemPreUpdate, + gz::sim::ISystemReset +) \ No newline at end of file From 7f22da71466993bda6629c4c6aef01e6a2f9322b Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 14:28:05 +0200 Subject: [PATCH 28/32] Fix launcher --- Launchers/package_delivery.launch.py | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/Launchers/package_delivery.launch.py b/Launchers/package_delivery.launch.py index f5414ed0a..0fba2865e 100644 --- a/Launchers/package_delivery.launch.py +++ b/Launchers/package_delivery.launch.py @@ -35,15 +35,6 @@ 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 - ) - gz_ros2_bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", @@ -52,6 +43,7 @@ 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( From deb347c7f31f948deaaa4bd2a4dbf836fc380749 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Wed, 22 Jul 2026 15:58:00 +0200 Subject: [PATCH 29/32] Fix rebase error --- CustomRobots/quadrotor/launch/quadrotor.launch.py | 2 +- .../quadrotor/models/quadrotor/quadrotor_common.urdf.xacro | 2 +- database/worlds.sql | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CustomRobots/quadrotor/launch/quadrotor.launch.py b/CustomRobots/quadrotor/launch/quadrotor.launch.py index 3b5c89d74..be4e0c04f 100644 --- a/CustomRobots/quadrotor/launch/quadrotor.launch.py +++ b/CustomRobots/quadrotor/launch/quadrotor.launch.py @@ -50,8 +50,8 @@ def launch_setup(context): xacro_file, mappings={ "camera": "true" if sensor == "camera" else "false", - "namespace": namespace, "gripper": gripper, + "namespace": namespace, }, ).toxml() diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro index c34e77531..d83f9aa8f 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro @@ -30,7 +30,7 @@ - + diff --git a/database/worlds.sql b/database/worlds.sql index ec0c4098d..f54c2cfdf 100644 --- a/database/worlds.sql +++ b/database/worlds.sql @@ -333,7 +333,7 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config, model_path 28 Dingo Low Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=low namespace:=do150 dingo/model/dingo/dingo.urdf.xacro 29 Dingo Medium Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=med namespace:=do150 dingo/model/dingo/dingo.urdf.xacro 30 Dingo High Noise /home/ws/src/CustomRobots/dingo/launch/dingo.launch.py do150 noise:=high namespace:=do150 dingo/model/dingo/dingo.urdf.xacro -31 Quadrotor Magnet /home/ws/src/CustomRobots/dingquadrotoro/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone gripper:=true quadrotor/models/quadrotor/quadrotor.urdf.xacro +31 Quadrotor Magnet /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone gripper:=true quadrotor/models/quadrotor/quadrotor.urdf.xacro \. -- From 4e7f8c47341f5c2b9fdef8cc7eb6594382aa9c85 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Thu, 23 Jul 2026 07:59:32 +0200 Subject: [PATCH 30/32] Add comments --- Industrial/drone_gripper/src/drone_gripper.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 58bcebca6..35f0b72eb 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -189,6 +189,7 @@ void PreUpdate( // 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. + //TODO: review if (this->activeJoint != kNullEntity && this->GripperGoneOrRemoving(_ecm)) { this->HandleResetDetach(_ecm); @@ -196,6 +197,7 @@ void PreUpdate( if (_info.paused) { + // TODO: wrong // A reset pauses the world before removing/resetting the drone. Detach the // box (so it is not jointed to the drone when it is removed) AND de-energize // the magnet. De-energizing is key: otherwise TryAttach would auto-re-attach From 28909b799e7f8b8edf6800569dda86cec584d03c Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 27 Jul 2026 20:52:47 +0200 Subject: [PATCH 31/32] Remove pause reset --- Industrial/drone_gripper/src/drone_gripper.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/Industrial/drone_gripper/src/drone_gripper.cpp b/Industrial/drone_gripper/src/drone_gripper.cpp index 35f0b72eb..8d3ffde1e 100644 --- a/Industrial/drone_gripper/src/drone_gripper.cpp +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -197,7 +197,7 @@ void PreUpdate( if (_info.paused) { - // TODO: wrong + // TODO: commented because it detaches when the user presses pause // A reset pauses the world before removing/resetting the drone. Detach the // box (so it is not jointed to the drone when it is removed) AND de-energize // the magnet. De-energizing is key: otherwise TryAttach would auto-re-attach @@ -205,12 +205,12 @@ void PreUpdate( // never grabs again. The exercise re-energizes the magnet when it wants to // grab, so a fresh run works normally. If GripperGoneOrRemoving already // handled it above this cycle, activeJoint is already null here. - if (this->activeJoint != kNullEntity) - this->HandleResetDetach(_ecm); - { - std::lock_guard lock(this->mutex); - this->magnetEnabled = false; - } + // if (this->activeJoint != kNullEntity) + // this->HandleResetDetach(_ecm); + // { + // std::lock_guard lock(this->mutex); + // this->magnetEnabled = false; + // } return; } From b9b82c1309a8621c25d633bc9ee9fd1db1ebcd55 Mon Sep 17 00:00:00 2001 From: Javier Izquierdo Hernandez Date: Mon, 27 Jul 2026 23:11:52 +0200 Subject: [PATCH 32/32] Linter --- Launchers/package_delivery.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Launchers/package_delivery.launch.py b/Launchers/package_delivery.launch.py index 0fba2865e..203717032 100644 --- a/Launchers/package_delivery.launch.py +++ b/Launchers/package_delivery.launch.py @@ -43,7 +43,7 @@ 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(