diff --git a/CustomRobots/quadrotor/launch/quadrotor.launch.py b/CustomRobots/quadrotor/launch/quadrotor.launch.py index b9d2023c0..8f1a814ac 100644 --- a/CustomRobots/quadrotor/launch/quadrotor.launch.py +++ b/CustomRobots/quadrotor/launch/quadrotor.launch.py @@ -25,6 +25,7 @@ def launch_setup(context): gz_sensor = LaunchConfiguration("sensor") gz_namespace = LaunchConfiguration("namespace") gz_entity = LaunchConfiguration("entity") + gz_gripper = LaunchConfiguration("gripper") gz_color = LaunchConfiguration("color") package_dir = get_package_share_directory("custom_robots") @@ -34,6 +35,7 @@ def launch_setup(context): sensor = gz_sensor.perform(context) namespace = gz_namespace.perform(context) entity = gz_entity.perform(context) + gripper = gz_gripper.perform(context) color = gz_color.perform(context) # ========================= @@ -50,6 +52,7 @@ def launch_setup(context): xacro_file, mappings={ "camera": "true" if sensor == "camera" else "false", + "gripper": gripper, "namespace": namespace, "color": color, }, @@ -172,6 +175,7 @@ def generate_launch_description(): DeclareLaunchArgument("P", default_value="0"), DeclareLaunchArgument("Y", default_value="0"), DeclareLaunchArgument("sensor", default_value="camera"), + DeclareLaunchArgument("gripper", default_value="false"), DeclareLaunchArgument("namespace", default_value="drone"), DeclareLaunchArgument("entity", default_value="drone"), DeclareLaunchArgument("color", default_value=""), diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro index dba755d8c..02a3a1b30 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor.urdf.xacro @@ -3,15 +3,18 @@ xmlns:xacro="http://ros.org/wiki/xacro"> + + + - + diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro index 47f2decf9..368fd6fd7 100644 --- a/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro +++ b/CustomRobots/quadrotor/models/quadrotor/quadrotor_common.urdf.xacro @@ -10,6 +10,25 @@ + + + + + + + + + + + + + + + + + + @@ -62,7 +81,7 @@ - + @@ -113,6 +132,24 @@ + + + + + + + + + + + + + + + + + diff --git a/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro b/CustomRobots/quadrotor/models/quadrotor/quadrotor_gz.urdf.xacro index afbb90cd3..bb7e62882 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.2 + /${namespace}/gripper/magnet + /${namespace}/gripper/attached + /${namespace}/gripper/graspable + + + + 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..8d3ffde1e --- /dev/null +++ b/Industrial/drone_gripper/src/drone_gripper.cpp @@ -0,0 +1,425 @@ +// 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 +#include + +using namespace gz; +using namespace sim; + +namespace drone_gripper +{ + +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(); +} + +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(); + }); + + 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; + } + 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. + //TODO: review + if (this->activeJoint != kNullEntity && this->GripperGoneOrRemoving(_ecm)) + { + this->HandleResetDetach(_ecm); + } + + if (_info.paused) + { + // 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 + // 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; + } + + 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; + } + 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; + } + 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; + + 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}; + +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}; + +}; + +} + +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 dea0f828f..203717032 100644 --- a/Launchers/package_delivery.launch.py +++ b/Launchers/package_delivery.launch.py @@ -44,7 +44,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) ld.add_action(gz_ros2_bridge) diff --git a/database/worlds.sql b/database/worlds.sql index e1c1f0fd8..5d3ab3a61 100644 --- a/database/worlds.sql +++ b/database/worlds.sql @@ -243,7 +243,7 @@ COPY public.worlds_robots (id, world_id, robot_id, poses) FROM stdin; 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 11 {{-1.0,-4.0,0.3,0,0,1.5729}} +31 57 33 {{-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}} @@ -347,6 +347,7 @@ COPY public.robots (id, name, launch_file_path, entity, extra_config, model_path 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 Cat /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone color:=Orange quadrotor/models/quadrotor/quadrotor.urdf.xacro 32 Quadrotor Mouse /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone_mouse sensor:=camera namespace:=drone_mouse color:=Magenta quadrotor/models/quadrotor/quadrotor.urdf.xacro +33 Quadrotor Magnet /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py drone sensor:=camera namespace:=drone gripper:=true quadrotor/models/quadrotor/quadrotor.urdf.xacro \. --