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..619cf183f 100644
--- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro
+++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro
@@ -3,16 +3,51 @@
xmlns:xacro="http://ros.org/wiki/xacro">
+
+
-
+
+<<<<<<< HEAD
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ base_link
+ 0.15
+ /${namespace}/gripper/magnet
+ /${namespace}/gripper/attached
+ /${namespace}/gripper/graspable
+
+
+
+=======
+>>>>>>> upstream/drone-amazon-delivery
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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..77f104762
--- /dev/null
+++ b/Industrial/drone_gripper/src/drone_gripper.cpp
@@ -0,0 +1,344 @@
+// Drone magnetic gripper system plugin for gz-sim (Harmonic).
+//
+// 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):
+// 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
+
+using namespace gz;
+using namespace sim;
+
+namespace drone_gripper
+{
+
+class DroneGripper :
+ public System,
+ public ISystemConfigure,
+ public ISystemPreUpdate,
+ public ISystemReset
+{
+
+public:
+
+DroneGripper() = default;
+
+~DroneGripper()
+{
+ 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);
+ }
+
+ 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 (rclcpp::ok())
+ {
+ this->executor->spin_some();
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ }
+ });
+
+ std::cout << "[DroneGripper] Configured on model " << modelName
+ << " link=" << this->gripperLinkName
+ << " attach_distance=" << this->attachDistance << std::endl;
+}
+
+void PreUpdate(
+ const UpdateInfo &_info,
+ EntityComponentManager &_ecm) override
+{
+ // 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);
+ this->activeJoint = kNullEntity;
+ }
+
+ 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);
+ }
+}
+
+// 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)
+{
+ 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};
+
+};
+
+}
+
+GZ_ADD_PLUGIN(
+ drone_gripper::DroneGripper,
+ gz::sim::System,
+ gz::sim::ISystemConfigure,
+ gz::sim::ISystemPreUpdate,
+ gz::sim::ISystemReset
+)
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/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
\.
--