From eb49fd1b302d9187a8bd7760f9fe4e27f362063c Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Tue, 8 Jul 2025 16:55:29 +0530 Subject: [PATCH 01/13] init Signed-off-by: Adarsh Karan Kesavadas Prasanth --- src/systems/CMakeLists.txt | 1 + .../dynamic_detachable_joint/CMakeLists.txt | 6 + .../DynamicDetachableJoint.cc | 360 ++++++++++++++++++ .../DynamicDetachableJoint.hh | 165 ++++++++ 4 files changed, 532 insertions(+) create mode 100644 src/systems/dynamic_detachable_joint/CMakeLists.txt create mode 100644 src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc create mode 100644 src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 2076009df7..a7c53446ce 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -111,6 +111,7 @@ add_subdirectory(comms_endpoint) add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) +add_subdirectory(dynamic_detachable_joint) add_subdirectory(diff_drive) add_subdirectory(drive_to_pose_controller) add_subdirectory(dvl) diff --git a/src/systems/dynamic_detachable_joint/CMakeLists.txt b/src/systems/dynamic_detachable_joint/CMakeLists.txt new file mode 100644 index 0000000000..07b74ef1e0 --- /dev/null +++ b/src/systems/dynamic_detachable_joint/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(dynamic-detachable-joint + SOURCES + DynamicDetachableJoint.cc + PUBLIC_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} +) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc new file mode 100644 index 0000000000..6210970823 --- /dev/null +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -0,0 +1,360 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +// Author: Adarsh Karan K P, Neobotix GmbH + +#include + +#include +#include + +#include + +#include + +#include "gz/sim/components/DetachableJoint.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" + +#include "DynamicDetachableJoint.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +///////////////////////////////////////////////// +void DynamicDetachableJoint::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->model = Model(_entity); + if (!this->model.Valid(_ecm)) + { + gzerr << "DynamicDetachableJoint should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + if (_sdf->HasElement("parent_link")) + { + auto parentLinkName = _sdf->Get("parent_link"); + this->parentLinkEntity = this->model.LinkByName(_ecm, parentLinkName); + if (kNullEntity == this->parentLinkEntity) + { + gzerr << "Link with name " << parentLinkName + << " not found in model " << this->model.Name(_ecm) + << ". Make sure the parameter 'parent_link' has the " + << "correct value. Failed to initialize.\n"; + return; + } + } + else + { + gzerr << "'parent_link' is a required parameter for DynamicDetachableJoint. " + "Failed to initialize.\n"; + return; + } + + // Setup attach distance threshold + auto [value, found] = _sdf->Get("attach_distance", this->defaultAttachDistance); + if(!found) + { + gzmsg << "No 'attach_distance' specified in sdf, using default value of " + << this->defaultAttachDistance << " meters.\n"; + } + else + { + gzmsg << "Found 'attach_distance' in sdf: " << value << " meters.\n"; + } + + this->attachDistance = value; + gzmsg << "Final attachDistance set to: " << this->attachDistance << " meters.\n"; + + // Setup service + // Check if the SDF has a service_name element + std::vector serviceNames; + if (_sdf->HasElement("service_name")) + { + // If it does, add it to the list of service names + serviceNames.push_back(_sdf->Get("service_name")); + } + // Add a fallback service name + serviceNames.push_back("/model/" + this->model.Name(_ecm) + + "/dynamic_detachable_joint/attach_detach"); + + // Get the valid service name + this->serviceName = validTopic(serviceNames); + if (this->serviceName.empty()) + { + gzerr << "No valid service name for DynamicDetachableJoint could be found.\n"; + return; + } + gzdbg << "using service: " + << this->serviceName << std::endl; + + // Advertise the service + if (!this->node.Advertise(this->serviceName, &DynamicDetachableJoint::OnServiceRequest, this)) + { + std::cerr << "Error advertising service [" << serviceName << "]" << std::endl; + return; + } + + // Setup output topic + std::vector outputTopics; + if (_sdf->HasElement("output_topic")) + { + outputTopics.push_back(_sdf->Get("output_topic")); + } + + outputTopics.push_back("/model/" + this->model.Name(_ecm) + + "/detachable_joint/state"); + + this->outputTopic = validTopic(outputTopics); + if (this->outputTopic.empty()) + { + gzerr << "No valid output topics for DynamicDetachableJoint could be found.\n"; + return; + } + gzdbg << "Output topic is: " << this->outputTopic << std::endl; + + // Setup publisher for output topic + this->outputPub = this->node.Advertise( + this->outputTopic); + if (!this->outputPub) + { + gzerr << "Error advertising topic [" << this->outputTopic << "]" + << std::endl; + return; + } + + // Suppress Child Warning + this->suppressChildWarning = + _sdf->Get("suppress_child_warning", this->suppressChildWarning) + .first; + + this->validConfig = true; +} + +////////////////////////////////////////////////// +void DynamicDetachableJoint::PreUpdate( + const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("DynamicDetachableJoint::PreUpdate"); + // only allow attaching if child entity is detached + if (this->validConfig && !this->isAttached) + { + // return if attach is not requested. + if (!this->attachRequested){ + return; + } + // Look for the child model and link + Entity modelEntity{kNullEntity}; + + // if child model is the parent model + if ("__model__" == this->childModelName) + { + modelEntity = this->model.Entity(); + } + else + { + // Querying the ECM for the child model entity + modelEntity = _ecm.EntityByComponents( + components::Model(), components::Name(this->childModelName)); + } + if (kNullEntity != modelEntity) + { + this->childLinkEntity = _ecm.EntityByComponents( + components::Link(), + components::ParentEntity(modelEntity), + components::Name(this->childLinkName)); + + if (kNullEntity != this->childLinkEntity) + { + + // store the child and parent link poses in the world frame + math::Pose3d child_pose = gz::sim::worldPose(this->childLinkEntity, _ecm); + math::Pose3d parent_pose = gz::sim::worldPose(this->parentLinkEntity, _ecm); + + auto dist = child_pose.Pos().Distance(parent_pose.Pos()); + std::cout << "Centre-to-centre distance: " << dist << " m\n"; + + // Check if the child link is within the attach distance + if (dist > this->attachDistance) + { + std::cout << "Child Link " << this->childLinkName + << " is too far away from Parent" + << "Distance: " << dist + << ", Attach Distance: " << this->attachDistance + << ". Cannot attach.\n"; + this->attachRequested = false; // reset attach request + return; + } + // If the child link is within the attach distance, proceed to attach + std::cout << "Child Link " << this->childLinkName + << " is within attach distance of Parent Link. Proceeding to attach.\n"; + + // Attach the models + // We do this by creating a detachable joint entity. + this->detachableJointEntity = _ecm.CreateEntity(); + + // creating the joint + _ecm.CreateComponent( + this->detachableJointEntity, + components::DetachableJoint({this->parentLinkEntity, + this->childLinkEntity, "fixed"})); + this->attachRequested = false; + this->isAttached = true; + this->PublishJointState(this->isAttached); + gzdbg << "Attaching entity: " << this->detachableJointEntity + << std::endl; + } + else + { + gzwarn << "Child Link " << this->childLinkName + << " could not be found.\n"; + } + } + else if (!this->suppressChildWarning) + { + gzwarn << "Child Model " << this->childModelName + << " could not be found.\n"; + } + } + + // only allow detaching if child entity is attached + if (this->isAttached) + { + if (this->detachRequested && (kNullEntity != this->detachableJointEntity)) + { + // Detach the models + gzdbg << "Removing entity: " << this->detachableJointEntity << std::endl; + _ecm.RequestRemoveEntity(this->detachableJointEntity); + this->detachableJointEntity = kNullEntity; + this->detachRequested = false; + this->isAttached = false; + this->PublishJointState(this->isAttached); + } + } +} + +////////////////////////////////////////////////// +bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, + gz::msgs::AttachDetachResponse &_res) +{ + GZ_PROFILE("DynamicDetachableJoint::OnServiceRequest"); + + // Check if the request is valid + if (_req.child_model_name().empty() || _req.child_link_name().empty() ) + { + _res.set_success(false); + _res.set_message("Invalid request: child_model_name and child_link_name must be set."); + return true; + } + + if (_req.command().empty()) + { + _res.set_success(false); + _res.set_message("Invalid request: command must be 'attach' or 'detach'."); + return true; + } + // Set the child model and link names + this->childModelName = _req.child_model_name(); + this->childLinkName = _req.child_link_name(); + + // If attach is requested + if (_req.command() == "attach") + { + if (this->isAttached) + { + _res.set_success(false); + _res.set_message("Already attached to another object"); + gzdbg << "Already attached" << std::endl; + return true; + } + + this->OnAttachRequest(msgs::Empty()); + _res.set_success(true); + _res.set_message("Attach request received."); + } + + // If detach is requested + else if (_req.command() == "detach") + { + if (!this->isAttached) + { + _res.set_success(false); + _res.set_message("Already detached"); + gzdbg << "Already detached" << std::endl; + return true; + } + + this->OnDetachRequest(msgs::Empty()); + _res.set_success(true); + _res.set_message("Detach request processed successfully."); + } + + else + { + _res.set_success(false); + _res.set_message("Invalid command. Use 'attach' or 'detach'."); + return true; + } + return true; +} + +////////////////////////////////////////////////// +void DynamicDetachableJoint::OnAttachRequest(const msgs::Empty &) +{ + this->attachRequested = true; +} + +////////////////////////////////////////////////// +void DynamicDetachableJoint::OnDetachRequest(const msgs::Empty &) +{ + this->detachRequested = true; +} + +////////////////////////////////////////////////// +void DynamicDetachableJoint::PublishJointState(bool attached) +{ + gz::msgs::StringMsg detachedStateMsg; + if (attached) + { + detachedStateMsg.set_data("attached to " + + this->childModelName + " at link " + this->childLinkName); + } + else + { + detachedStateMsg.set_data("detached from " + + this->childModelName + " at link " + this->childLinkName); + } + this->outputPub.Publish(detachedStateMsg); +} + +GZ_ADD_PLUGIN(DynamicDetachableJoint, + System, + DynamicDetachableJoint::ISystemConfigure, + DynamicDetachableJoint::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(DynamicDetachableJoint, + "gz::sim::systems::DynamicDetachableJoint") diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh new file mode 100644 index 0000000000..705d251712 --- /dev/null +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -0,0 +1,165 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +// Author: Adarsh Karan K P, Neobotix GmbH + +#ifndef GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_ +#define GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_ + +#include + +#include +#include +#include + +#include "gz/sim/Model.hh" +#include "gz/sim/System.hh" + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + /// \brief A system that initially attaches two models via a fixed joint and + /// allows for the models to get detached during simulation via a topic. A + /// model can be re-attached during simulation via a topic. The status of the + /// detached state can be monitored via a topic as well. + /// + /// ## System Parameters + /// + /// - ``: Name of the link in the parent model to be used in + /// creating a fixed joint with a link in the child model. + /// + /// - ``: Name of the model to which this model will be connected + /// + /// - ``: Name of the link in the child model to be used in + /// creating a fixed joint with a link in the parent model. + /// + /// - `` (optional): Topic name to be used for detaching connections. + /// Using is preferred. + /// + /// - `` (optional): Topic name to be used for detaching + /// connections. If multiple detachable plugin is used in one model, + /// `detach_topic` is REQUIRED to detach child models individually. + /// + /// - `` (optional): Topic name to be used for attaching + /// connections. If multiple detachable plugin is used in one model, + /// `attach_topic` is REQUIRED to attach child models individually. + /// + /// - `` (optional): Topic name to be used for publishing + /// the state of the detachment. If multiple detachable plugin is used in + /// one model, `output_topic` is REQUIRED to publish child models state + /// individually. + /// + /// - `` (optional): If true, the system + /// will not print a warning message if a child model does not exist yet. + /// Otherwise, a warning message is printed. Defaults to false. + + class DynamicDetachableJoint + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// Documentation inherited + public: DynamicDetachableJoint() = default; + + /// Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + /// Documentation inherited + public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + /// \brief Gazebo communication node. + private: transport::Node node; + + /// \brief A publisher to send state of the detachment + private: transport::Node::Publisher outputPub; + + /// \brief Callback for attach/detach service request + private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, + gz::msgs::AttachDetachResponse &_res); + + /// \brief Helper function to publish the state of the detachment + private: void PublishJointState(bool attached); + + /// \brief Toggle the attach state. + private: void OnAttachRequest(const msgs::Empty &_msg); + + /// \brief Toggle the detach state. + private: void OnDetachRequest(const msgs::Empty &_msg); + + /// \brief The model associated with this system. + private: Model model; + + /// \brief Name of child model + private: std::string childModelName; + + /// \brief Name of attachment link in the child model + private: std::string childLinkName; + + /// \brief Pose of the child link in world frame + const components::WorldPose* childPoseInWorld; + + /// \brief Minimum distance to consider the child link as attached + private: double defaultAttachDistance = 0.1; + + /// \brief Distance to consider the child link as attached + private: double attachDistance{defaultAttachDistance}; + /// \brief Service name to be used for attaching or detaching connections + private: std::string serviceName; + + /// \brief Topic to be used for publishing detached state + private: std::string outputTopic; + + /// \brief Whether to suppress warning about missing child model. + private: bool suppressChildWarning{false}; + + /// \brief Entity of attachment link in the parent model + private: Entity parentLinkEntity{kNullEntity}; + + /// \brief Entity of attachment link in the child model + private: Entity childLinkEntity{kNullEntity}; + + /// \brief Entity of the detachable joint created by this system + private: Entity detachableJointEntity{kNullEntity}; + + /// \brief Whether detachment has been requested + private: std::atomic detachRequested{false}; + + /// \brief Whether attachment has been requested + private: std::atomic attachRequested{false}; + + /// \brief Whether child entity is attached + private: std::atomic isAttached{false}; + + /// \brief Whether all parameters are valid and the system can proceed + private: bool validConfig{false}; + + }; + } +} +} +} + +#endif From adf8da1b09e9e75b62e70f3f26368672075f035a Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 11 Sep 2025 18:19:51 +0530 Subject: [PATCH 02/13] better logs and comments Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 62 ++++++++-------- .../DynamicDetachableJoint.hh | 70 ++++++++----------- 2 files changed, 61 insertions(+), 71 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index 6210970823..1f8e880e17 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2025 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -13,8 +13,9 @@ * See the License for the specific language governing permissions and * limitations under the License. * + * Author: Adarsh Karan K P, Neobotix GmbH + * */ -// Author: Adarsh Karan K P, Neobotix GmbH #include @@ -42,9 +43,9 @@ using namespace systems; ///////////////////////////////////////////////// void DynamicDetachableJoint::Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) { this->model = Model(_entity); if (!this->model.Valid(_ecm)) @@ -76,7 +77,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, // Setup attach distance threshold auto [value, found] = _sdf->Get("attach_distance", this->defaultAttachDistance); - if(!found) + if (!found) { gzmsg << "No 'attach_distance' specified in sdf, using default value of " << this->defaultAttachDistance << " meters.\n"; @@ -93,7 +94,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, // Check if the SDF has a service_name element std::vector serviceNames; if (_sdf->HasElement("service_name")) - { + { // If it does, add it to the list of service names serviceNames.push_back(_sdf->Get("service_name")); } @@ -114,7 +115,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, // Advertise the service if (!this->node.Advertise(this->serviceName, &DynamicDetachableJoint::OnServiceRequest, this)) { - std::cerr << "Error advertising service [" << serviceName << "]" << std::endl; + gzerr << "Error advertising service [" << this->serviceName << "]" << std::endl; return; } @@ -189,30 +190,27 @@ void DynamicDetachableJoint::PreUpdate( components::Name(this->childLinkName)); if (kNullEntity != this->childLinkEntity) - { - - // store the child and parent link poses in the world frame - math::Pose3d child_pose = gz::sim::worldPose(this->childLinkEntity, _ecm); - math::Pose3d parent_pose = gz::sim::worldPose(this->parentLinkEntity, _ecm); - - auto dist = child_pose.Pos().Distance(parent_pose.Pos()); - std::cout << "Centre-to-centre distance: " << dist << " m\n"; - - // Check if the child link is within the attach distance - if (dist > this->attachDistance) { - std::cout << "Child Link " << this->childLinkName - << " is too far away from Parent" - << "Distance: " << dist - << ", Attach Distance: " << this->attachDistance - << ". Cannot attach.\n"; - this->attachRequested = false; // reset attach request - return; - } - // If the child link is within the attach distance, proceed to attach - std::cout << "Child Link " << this->childLinkName - << " is within attach distance of Parent Link. Proceeding to attach.\n"; - + // store the child and parent link poses in the world frame + math::Pose3d childPose = gz::sim::worldPose(this->childLinkEntity, _ecm); + math::Pose3d parentPose = gz::sim::worldPose(this->parentLinkEntity, _ecm); + + auto dist = childPose.Pos().Distance(parentPose.Pos()); + gzdbg << "Centre-to-centre distance: " << dist << " m" << std::endl; + + // Check if the child link is within the attach distance + if (dist > this->attachDistance) + { + gzwarn << "Child Link [" << this->childLinkName + << "] is too far from parent. Distance: " << dist + << "m, threshold: " << this->attachDistance << "m" << std::endl; + this->attachRequested = false; // reset attach request + return; + } + // If the child link is within the attach distance, proceed to attach + gzdbg << "Child Link " << this->childLinkName + << " is within attach distance of Parent Link. Proceeding to attach." << std::endl; + // Attach the models // We do this by creating a detachable joint entity. this->detachableJointEntity = _ecm.CreateEntity(); @@ -262,7 +260,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques gz::msgs::AttachDetachResponse &_res) { GZ_PROFILE("DynamicDetachableJoint::OnServiceRequest"); - + // Check if the request is valid if (_req.child_model_name().empty() || _req.child_link_name().empty() ) { diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh index 705d251712..e77c8c4e34 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2025 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -13,8 +13,9 @@ * See the License for the specific language governing permissions and * limitations under the License. * + * Author: Adarsh Karan K P, Neobotix GmbH + * */ -// Author: Adarsh Karan K P, Neobotix GmbH #ifndef GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_ #define GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_ @@ -36,40 +37,31 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { - /// \brief A system that initially attaches two models via a fixed joint and - /// allows for the models to get detached during simulation via a topic. A - /// model can be re-attached during simulation via a topic. The status of the - /// detached state can be monitored via a topic as well. - /// - /// ## System Parameters - /// - /// - ``: Name of the link in the parent model to be used in - /// creating a fixed joint with a link in the child model. - /// - /// - ``: Name of the model to which this model will be connected - /// - /// - ``: Name of the link in the child model to be used in - /// creating a fixed joint with a link in the parent model. - /// - /// - `` (optional): Topic name to be used for detaching connections. - /// Using is preferred. - /// - /// - `` (optional): Topic name to be used for detaching - /// connections. If multiple detachable plugin is used in one model, - /// `detach_topic` is REQUIRED to detach child models individually. - /// - /// - `` (optional): Topic name to be used for attaching - /// connections. If multiple detachable plugin is used in one model, - /// `attach_topic` is REQUIRED to attach child models individually. - /// - /// - `` (optional): Topic name to be used for publishing - /// the state of the detachment. If multiple detachable plugin is used in - /// one model, `output_topic` is REQUIRED to publish child models state - /// individually. - /// - /// - `` (optional): If true, the system - /// will not print a warning message if a child model does not exist yet. - /// Otherwise, a warning message is printed. Defaults to false. +/// \brief A system that dynamically creates detachable joints between models +/// via a service interface, allowing runtime attachment and detachment of any +/// model with a parent model that is defined in the SDF/URDF. +/// +/// Unlike the standard DetachableJoint which requires child model specification +/// in the SDF, this system accepts child model and link names via service calls. +/// Hence, it is a more flexible extension of the existing DetachableJoint system. +/// +/// ## System Parameters +/// +/// - ``: Name of the link in the parent model to be used in +/// creating a fixed joint with a link in the child model. Required. +/// +/// - `` (optional): Maximum distance in meters between parent +/// and child links to allow attachment. Defaults to 0.1 meters. +/// +/// - `` (optional): Service name for attach/detach requests. +/// Defaults to `/model/{model_name}/dynamic_detachable_joint/attach_detach`. +/// +/// - `` (optional): Topic name for publishing attachment state. +/// Defaults to `/model/{model_name}/detachable_joint/state`. +/// +/// - `` (optional): If true, the system +/// will not print a warning message if a child model does not exist yet. +/// Otherwise, a warning message is printed. Defaults to false. class DynamicDetachableJoint : public System, @@ -97,6 +89,9 @@ namespace systems private: transport::Node::Publisher outputPub; /// \brief Callback for attach/detach service request + /// \param[in] _req Request message containing child model/link names and command + /// \param[out] _res Response message with success status and message + /// \return Always returns true to indicate message was processed private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, gz::msgs::AttachDetachResponse &_res); @@ -118,9 +113,6 @@ namespace systems /// \brief Name of attachment link in the child model private: std::string childLinkName; - /// \brief Pose of the child link in world frame - const components::WorldPose* childPoseInWorld; - /// \brief Minimum distance to consider the child link as attached private: double defaultAttachDistance = 0.1; From 33c1fd65d83735b3c071cc2f6ca50f4a84fbb26e Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 11 Sep 2025 19:49:56 +0530 Subject: [PATCH 03/13] fixed an edge case where detach did not obey the requested model name, added clearer loggin Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 101 +++++++++++------- .../DynamicDetachableJoint.hh | 4 + 2 files changed, 67 insertions(+), 38 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index 1f8e880e17..2f2d26b20c 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -222,6 +222,9 @@ void DynamicDetachableJoint::PreUpdate( this->childLinkEntity, "fixed"})); this->attachRequested = false; this->isAttached = true; + // Keep track of the attached pair for future validation + this->attachedChildModelName = this->childModelName; + this->attachedChildLinkName = this->childLinkName; this->PublishJointState(this->isAttached); gzdbg << "Attaching entity: " << this->detachableJointEntity << std::endl; @@ -250,7 +253,10 @@ void DynamicDetachableJoint::PreUpdate( this->detachableJointEntity = kNullEntity; this->detachRequested = false; this->isAttached = false; + // Publish using the last known attached pair, then clear them. this->PublishJointState(this->isAttached); + this->attachedChildModelName.clear(); + this->attachedChildLinkName.clear(); } } } @@ -275,50 +281,69 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques _res.set_message("Invalid request: command must be 'attach' or 'detach'."); return true; } - // Set the child model and link names - this->childModelName = _req.child_model_name(); - this->childLinkName = _req.child_link_name(); - // If attach is requested - if (_req.command() == "attach") - { - if (this->isAttached) + // If attach is requested + if (_req.command() == "attach") + { + if (this->isAttached) + { + _res.set_success(false); + _res.set_message("Already attached to child model " + this->attachedChildModelName + + " at link " + this->attachedChildLinkName + "."); + gzdbg << "Already attached" << std::endl; + return true; + } + + // set the child model and link names from the request + this->childModelName = _req.child_model_name(); + this->childLinkName = _req.child_link_name(); + this->OnAttachRequest(msgs::Empty()); + _res.set_success(true); + _res.set_message("Attached to child model " + this->childModelName + + " at link " + this->childLinkName + "."); + } + + // If detach is requested + else if (_req.command() == "detach") + { + if (!this->isAttached) + { + _res.set_success(false); + _res.set_message(std::string("Detach request received for ") + + this->attachedChildModelName + "/" + this->attachedChildLinkName); + gzdbg << "Already detached" << std::endl; + return true; + } + + // Validate that the request matches what is actually attached. + const auto &reqModel = _req.child_model_name(); + const auto &reqLink = _req.child_link_name(); + if (reqModel != this->attachedChildModelName || + reqLink != this->attachedChildLinkName) { _res.set_success(false); - _res.set_message("Already attached to another object"); - gzdbg << "Already attached" << std::endl; + _res.set_message( + "Detach rejected: requested " + reqModel + "/" + reqLink + + " but currently attached to " + this->attachedChildModelName + "/" + + this->attachedChildLinkName + "." + ); return true; } - this->OnAttachRequest(msgs::Empty()); - _res.set_success(true); - _res.set_message("Attach request received."); - } - - // If detach is requested - else if (_req.command() == "detach") - { - if (!this->isAttached) - { - _res.set_success(false); - _res.set_message("Already detached"); - gzdbg << "Already detached" << std::endl; - return true; - } - - this->OnDetachRequest(msgs::Empty()); - _res.set_success(true); - _res.set_message("Detach request processed successfully."); - } - - else - { - _res.set_success(false); - _res.set_message("Invalid command. Use 'attach' or 'detach'."); - return true; - } - return true; -} + this->OnDetachRequest(msgs::Empty()); + _res.set_success(true); + _res.set_message("Detached from child model " + this->attachedChildModelName + + " at link " + this->attachedChildLinkName + "."); + } + + else + { + _res.set_success(false); + _res.set_message("Invalid command. Use 'attach' or 'detach'."); + return true; + } + return true; + } ////////////////////////////////////////////////// void DynamicDetachableJoint::OnAttachRequest(const msgs::Empty &) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh index e77c8c4e34..691c436104 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -113,6 +113,10 @@ namespace systems /// \brief Name of attachment link in the child model private: std::string childLinkName; + /// \brief Variables to keep track of what is currently attached + private: std::string attachedChildModelName; + private: std::string attachedChildLinkName; + /// \brief Minimum distance to consider the child link as attached private: double defaultAttachDistance = 0.1; From 41341658ec02c24fcaafcadd1f0e08a44de90ec8 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Wed, 17 Sep 2025 13:26:47 +0530 Subject: [PATCH 04/13] fixed indentation and headers Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 37 ++++++++++--------- .../DynamicDetachableJoint.hh | 8 +++- 2 files changed, 27 insertions(+), 18 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index 2f2d26b20c..cb77e39d75 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -17,13 +17,15 @@ * */ +#include "DynamicDetachableJoint.hh" + +#include #include +#include #include #include -#include - #include #include "gz/sim/components/DetachableJoint.hh" @@ -32,11 +34,12 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Pose.hh" + +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" #include "gz/sim/Model.hh" #include "gz/sim/Util.hh" -#include "DynamicDetachableJoint.hh" - using namespace gz; using namespace sim; using namespace systems; @@ -109,8 +112,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, gzerr << "No valid service name for DynamicDetachableJoint could be found.\n"; return; } - gzdbg << "using service: " - << this->serviceName << std::endl; + gzdbg << "using service: " << this->serviceName << std::endl; // Advertise the service if (!this->node.Advertise(this->serviceName, &DynamicDetachableJoint::OnServiceRequest, this)) @@ -165,7 +167,8 @@ void DynamicDetachableJoint::PreUpdate( if (this->validConfig && !this->isAttached) { // return if attach is not requested. - if (!this->attachRequested){ + if (!this->attachRequested) + { return; } // Look for the child model and link @@ -202,14 +205,14 @@ void DynamicDetachableJoint::PreUpdate( if (dist > this->attachDistance) { gzwarn << "Child Link [" << this->childLinkName - << "] is too far from parent. Distance: " << dist - << "m, threshold: " << this->attachDistance << "m" << std::endl; + << "] is too far from parent. Distance: " << dist + << "m, threshold: " << this->attachDistance << "m" << std::endl; this->attachRequested = false; // reset attach request return; } // If the child link is within the attach distance, proceed to attach gzdbg << "Child Link " << this->childLinkName - << " is within attach distance of Parent Link. Proceeding to attach." << std::endl; + << " is within attach distance of Parent Link. Proceeding to attach." << std::endl; // Attach the models // We do this by creating a detachable joint entity. @@ -227,18 +230,18 @@ void DynamicDetachableJoint::PreUpdate( this->attachedChildLinkName = this->childLinkName; this->PublishJointState(this->isAttached); gzdbg << "Attaching entity: " << this->detachableJointEntity - << std::endl; + << std::endl; } else { gzwarn << "Child Link " << this->childLinkName - << " could not be found.\n"; + << " could not be found.\n"; } } else if (!this->suppressChildWarning) { gzwarn << "Child Model " << this->childModelName - << " could not be found.\n"; + << " could not be found.\n"; } } @@ -263,7 +266,7 @@ void DynamicDetachableJoint::PreUpdate( ////////////////////////////////////////////////// bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, - gz::msgs::AttachDetachResponse &_res) + gz::msgs::AttachDetachResponse &_res) { GZ_PROFILE("DynamicDetachableJoint::OnServiceRequest"); @@ -289,7 +292,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques { _res.set_success(false); _res.set_message("Already attached to child model " + this->attachedChildModelName + - " at link " + this->attachedChildLinkName + "."); + " at link " + this->attachedChildLinkName + "."); gzdbg << "Already attached" << std::endl; return true; } @@ -300,7 +303,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques this->OnAttachRequest(msgs::Empty()); _res.set_success(true); _res.set_message("Attached to child model " + this->childModelName + - " at link " + this->childLinkName + "."); + " at link " + this->childLinkName + "."); } // If detach is requested @@ -333,7 +336,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques this->OnDetachRequest(msgs::Empty()); _res.set_success(true); _res.set_message("Detached from child model " + this->attachedChildModelName + - " at link " + this->attachedChildLinkName + "."); + " at link " + this->attachedChildLinkName + "."); } else diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh index 691c436104..781b6c50c6 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -21,11 +21,17 @@ #define GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_ #include +#include +#include #include #include + #include +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" + #include "gz/sim/Model.hh" #include "gz/sim/System.hh" @@ -93,7 +99,7 @@ namespace systems /// \param[out] _res Response message with success status and message /// \return Always returns true to indicate message was processed private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, - gz::msgs::AttachDetachResponse &_res); + gz::msgs::AttachDetachResponse &_res); /// \brief Helper function to publish the state of the detachment private: void PublishJointState(bool attached); From 3ec1affa2ee8fddfc237699acd848744c227d7b2 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 25 Sep 2025 10:15:30 +0530 Subject: [PATCH 05/13] more indentation fixes - nits Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index cb77e39d75..d5e3a9c9b5 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -112,7 +112,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, gzerr << "No valid service name for DynamicDetachableJoint could be found.\n"; return; } - gzdbg << "using service: " << this->serviceName << std::endl; + gzdbg << "Using service: " << this->serviceName << std::endl; // Advertise the service if (!this->node.Advertise(this->serviceName, &DynamicDetachableJoint::OnServiceRequest, this)) @@ -291,8 +291,8 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques if (this->isAttached) { _res.set_success(false); - _res.set_message("Already attached to child model " + this->attachedChildModelName + - " at link " + this->attachedChildLinkName + "."); + _res.set_message("Already attached to child model " + this->attachedChildModelName + + " at link " + this->attachedChildLinkName + "."); gzdbg << "Already attached" << std::endl; return true; } @@ -300,10 +300,10 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques // set the child model and link names from the request this->childModelName = _req.child_model_name(); this->childLinkName = _req.child_link_name(); - this->OnAttachRequest(msgs::Empty()); - _res.set_success(true); - _res.set_message("Attached to child model " + this->childModelName + - " at link " + this->childLinkName + "."); + this->OnAttachRequest(msgs::Empty()); + _res.set_success(true); + _res.set_message("Attached to child model " + this->childModelName + + " at link " + this->childLinkName + "."); } // If detach is requested From 6474e1236d74e18c1679ba457d1f13c6ce931053 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 25 Sep 2025 10:33:34 +0530 Subject: [PATCH 06/13] removed unwanted dependency on Empty proto Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 16 ++-------------- .../DynamicDetachableJoint.hh | 7 ------- 2 files changed, 2 insertions(+), 21 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index d5e3a9c9b5..0037742e14 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -300,7 +300,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques // set the child model and link names from the request this->childModelName = _req.child_model_name(); this->childLinkName = _req.child_link_name(); - this->OnAttachRequest(msgs::Empty()); + this->attachRequested = true; _res.set_success(true); _res.set_message("Attached to child model " + this->childModelName + " at link " + this->childLinkName + "."); @@ -333,7 +333,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques return true; } - this->OnDetachRequest(msgs::Empty()); + this->detachRequested = true; _res.set_success(true); _res.set_message("Detached from child model " + this->attachedChildModelName + " at link " + this->attachedChildLinkName + "."); @@ -348,18 +348,6 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques return true; } -////////////////////////////////////////////////// -void DynamicDetachableJoint::OnAttachRequest(const msgs::Empty &) -{ - this->attachRequested = true; -} - -////////////////////////////////////////////////// -void DynamicDetachableJoint::OnDetachRequest(const msgs::Empty &) -{ - this->detachRequested = true; -} - ////////////////////////////////////////////////// void DynamicDetachableJoint::PublishJointState(bool attached) { diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh index 781b6c50c6..575a15da22 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -21,7 +21,6 @@ #define GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_ #include -#include #include #include @@ -104,12 +103,6 @@ namespace systems /// \brief Helper function to publish the state of the detachment private: void PublishJointState(bool attached); - /// \brief Toggle the attach state. - private: void OnAttachRequest(const msgs::Empty &_msg); - - /// \brief Toggle the detach state. - private: void OnDetachRequest(const msgs::Empty &_msg); - /// \brief The model associated with this system. private: Model model; From 15793fe6e7f2ba59215dad826a0c0fa337b98785 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 25 Sep 2025 12:18:54 +0530 Subject: [PATCH 07/13] used error first approach for better readability Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 108 +++++++++--------- 1 file changed, 56 insertions(+), 52 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index 0037742e14..a2525aaf57 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -129,7 +129,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, } outputTopics.push_back("/model/" + this->model.Name(_ecm) + - "/detachable_joint/state"); + "/dynamic_detachable_joint/state"); this->outputTopic = validTopic(outputTopics); if (this->outputTopic.empty()) @@ -185,64 +185,68 @@ void DynamicDetachableJoint::PreUpdate( modelEntity = _ecm.EntityByComponents( components::Model(), components::Name(this->childModelName)); } - if (kNullEntity != modelEntity) - { - this->childLinkEntity = _ecm.EntityByComponents( - components::Link(), - components::ParentEntity(modelEntity), - components::Name(this->childLinkName)); - if (kNullEntity != this->childLinkEntity) - { - // store the child and parent link poses in the world frame - math::Pose3d childPose = gz::sim::worldPose(this->childLinkEntity, _ecm); - math::Pose3d parentPose = gz::sim::worldPose(this->parentLinkEntity, _ecm); - - auto dist = childPose.Pos().Distance(parentPose.Pos()); - gzdbg << "Centre-to-centre distance: " << dist << " m" << std::endl; - - // Check if the child link is within the attach distance - if (dist > this->attachDistance) - { - gzwarn << "Child Link [" << this->childLinkName - << "] is too far from parent. Distance: " << dist - << "m, threshold: " << this->attachDistance << "m" << std::endl; - this->attachRequested = false; // reset attach request - return; - } - // If the child link is within the attach distance, proceed to attach - gzdbg << "Child Link " << this->childLinkName - << " is within attach distance of Parent Link. Proceeding to attach." << std::endl; - - // Attach the models - // We do this by creating a detachable joint entity. - this->detachableJointEntity = _ecm.CreateEntity(); - - // creating the joint - _ecm.CreateComponent( - this->detachableJointEntity, - components::DetachableJoint({this->parentLinkEntity, - this->childLinkEntity, "fixed"})); - this->attachRequested = false; - this->isAttached = true; - // Keep track of the attached pair for future validation - this->attachedChildModelName = this->childModelName; - this->attachedChildLinkName = this->childLinkName; - this->PublishJointState(this->isAttached); - gzdbg << "Attaching entity: " << this->detachableJointEntity - << std::endl; - } - else + // if child model is not found + if (kNullEntity == modelEntity) + { + if (!this->suppressChildWarning) { - gzwarn << "Child Link " << this->childLinkName + gzwarn << "Child Model " << this->childModelName << " could not be found.\n"; } + return; + } + + this->childLinkEntity = _ecm.EntityByComponents( + components::Link(), + components::ParentEntity(modelEntity), + components::Name(this->childLinkName)); + + // if child link is not found + if (kNullEntity == this->childLinkEntity) + { + gzwarn << "Child Link " << this->childLinkName + << " could not be found.\n"; + return; } - else if (!this->suppressChildWarning) + + // store the child and parent link poses in the world frame + math::Pose3d childPose = gz::sim::worldPose(this->childLinkEntity, _ecm); + math::Pose3d parentPose = gz::sim::worldPose(this->parentLinkEntity, _ecm); + + auto dist = childPose.Pos().Distance(parentPose.Pos()); + gzdbg << "Centre-to-centre distance: " << dist << " m" << std::endl; + + // Check if the child link is within the attach distance + if (dist > this->attachDistance) { - gzwarn << "Child Model " << this->childModelName - << " could not be found.\n"; + gzwarn << "Child Link [" << this->childLinkName + << "] is too far from parent. Distance: " << dist + << "m, threshold: " << this->attachDistance << "m" << std::endl; + this->attachRequested = false; // reset attach request + return; } + // If the child link is within the attach distance, proceed to attach + gzdbg << "Child Link " << this->childLinkName + << " is within attach distance of Parent Link. Proceeding to attach." << std::endl; + + // Attach the models + // We do this by creating a detachable joint entity. + this->detachableJointEntity = _ecm.CreateEntity(); + + // creating the joint + _ecm.CreateComponent( + this->detachableJointEntity, + components::DetachableJoint({this->parentLinkEntity, + this->childLinkEntity, "fixed"})); + this->attachRequested = false; + this->isAttached = true; + // Keep track of the attached pair for future validation + this->attachedChildModelName = this->childModelName; + this->attachedChildLinkName = this->childLinkName; + this->PublishJointState(this->isAttached); + gzdbg << "Attaching entity: " << this->detachableJointEntity + << std::endl; } // only allow detaching if child entity is attached From 86eca081fc4b6d3f24076ab4dffd854022ba3f63 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 25 Sep 2025 12:57:29 +0530 Subject: [PATCH 08/13] publishing link entity instead of string msg# Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 14 +++++++------- .../DynamicDetachableJoint.hh | 1 + 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index a2525aaf57..3bfc17c49e 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -140,7 +140,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, gzdbg << "Output topic is: " << this->outputTopic << std::endl; // Setup publisher for output topic - this->outputPub = this->node.Advertise( + this->outputPub = this->node.Advertise( this->outputTopic); if (!this->outputPub) { @@ -355,18 +355,18 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques ////////////////////////////////////////////////// void DynamicDetachableJoint::PublishJointState(bool attached) { - gz::msgs::StringMsg detachedStateMsg; + gz::msgs::Entity stateMsg; if (attached) { - detachedStateMsg.set_data("attached to " + - this->childModelName + " at link " + this->childLinkName); + stateMsg.set_id(this->childLinkEntity); + stateMsg.set_type(gz::msgs::Entity::LINK); } else { - detachedStateMsg.set_data("detached from " + - this->childModelName + " at link " + this->childLinkName); + stateMsg.set_id(kNullEntity); + stateMsg.set_type(gz::msgs::Entity::NONE); } - this->outputPub.Publish(detachedStateMsg); + this->outputPub.Publish(stateMsg); } GZ_ADD_PLUGIN(DynamicDetachableJoint, diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh index 575a15da22..84828a32de 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -21,6 +21,7 @@ #define GZ_SIM_SYSTEMS_DYNAMICDETACHABLEJOINT_HH_ #include +#include #include #include From 36eaffae1e8e446bb9e7ca9af88ce2aaad937b78 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 25 Sep 2025 13:36:18 +0530 Subject: [PATCH 09/13] removed suppress child option Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 12 ++---------- .../DynamicDetachableJoint.hh | 7 ------- 2 files changed, 2 insertions(+), 17 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index 3bfc17c49e..bf1a55f3dd 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -149,11 +149,6 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, return; } - // Suppress Child Warning - this->suppressChildWarning = - _sdf->Get("suppress_child_warning", this->suppressChildWarning) - .first; - this->validConfig = true; } @@ -189,11 +184,8 @@ void DynamicDetachableJoint::PreUpdate( // if child model is not found if (kNullEntity == modelEntity) { - if (!this->suppressChildWarning) - { - gzwarn << "Child Model " << this->childModelName - << " could not be found.\n"; - } + gzwarn << "Child Model " << this->childModelName + << " could not be found.\n"; return; } diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh index 84828a32de..a1953e21cc 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -64,10 +64,6 @@ namespace systems /// /// - `` (optional): Topic name for publishing attachment state. /// Defaults to `/model/{model_name}/detachable_joint/state`. -/// -/// - `` (optional): If true, the system -/// will not print a warning message if a child model does not exist yet. -/// Otherwise, a warning message is printed. Defaults to false. class DynamicDetachableJoint : public System, @@ -128,9 +124,6 @@ namespace systems /// \brief Topic to be used for publishing detached state private: std::string outputTopic; - /// \brief Whether to suppress warning about missing child model. - private: bool suppressChildWarning{false}; - /// \brief Entity of attachment link in the parent model private: Entity parentLinkEntity{kNullEntity}; From 9204cc48d4ceaee7db99fcf16b7ee007636cddc8 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 25 Sep 2025 16:45:37 +0530 Subject: [PATCH 10/13] added mutex lock guard instead of atomic Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../dynamic_detachable_joint/DynamicDetachableJoint.cc | 4 ++++ .../dynamic_detachable_joint/DynamicDetachableJoint.hh | 10 ++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index bf1a55f3dd..3aa3fcb4ff 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -19,6 +19,7 @@ #include "DynamicDetachableJoint.hh" +#include #include #include @@ -158,6 +159,8 @@ void DynamicDetachableJoint::PreUpdate( EntityComponentManager &_ecm) { GZ_PROFILE("DynamicDetachableJoint::PreUpdate"); + std::lock_guard lock(this->mutex); + // only allow attaching if child entity is detached if (this->validConfig && !this->isAttached) { @@ -265,6 +268,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques gz::msgs::AttachDetachResponse &_res) { GZ_PROFILE("DynamicDetachableJoint::OnServiceRequest"); + std::lock_guard lock(this->mutex); // Check if the request is valid if (_req.child_model_name().empty() || _req.child_link_name().empty() ) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh index a1953e21cc..8bee06876d 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.hh @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -134,17 +134,19 @@ namespace systems private: Entity detachableJointEntity{kNullEntity}; /// \brief Whether detachment has been requested - private: std::atomic detachRequested{false}; + private: bool detachRequested{false}; /// \brief Whether attachment has been requested - private: std::atomic attachRequested{false}; + private: bool attachRequested{false}; /// \brief Whether child entity is attached - private: std::atomic isAttached{false}; + private: bool isAttached{false}; /// \brief Whether all parameters are valid and the system can proceed private: bool validConfig{false}; + /// \brief Mutex to protect access to member variables + private: std::mutex mutex; }; } } From 99e2c62e757aa6b56703b8e4e17636c9ded4aa92 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 25 Sep 2025 16:48:14 +0530 Subject: [PATCH 11/13] updated debug msg Signed-off-by: Adarsh Karan Kesavadas Prasanth --- src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index 3aa3fcb4ff..eedacf91fb 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -293,7 +293,8 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques _res.set_success(false); _res.set_message("Already attached to child model " + this->attachedChildModelName + " at link " + this->attachedChildLinkName + "."); - gzdbg << "Already attached" << std::endl; + gzdbg << "Already attached to child model " << this->attachedChildModelName + << " at link " << this->attachedChildLinkName << std::endl; return true; } From fc1cb566585af0cebef3b726239f1691baf8b471 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Fri, 26 Sep 2025 11:18:49 +0530 Subject: [PATCH 12/13] improved logging, added reset for attachRequested flag Signed-off-by: Adarsh Karan Kesavadas Prasanth --- .../DynamicDetachableJoint.cc | 56 +++++++++++-------- 1 file changed, 32 insertions(+), 24 deletions(-) diff --git a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc index eedacf91fb..65405bd6aa 100644 --- a/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc +++ b/src/systems/dynamic_detachable_joint/DynamicDetachableJoint.cc @@ -83,7 +83,7 @@ void DynamicDetachableJoint::Configure(const Entity &_entity, auto [value, found] = _sdf->Get("attach_distance", this->defaultAttachDistance); if (!found) { - gzmsg << "No 'attach_distance' specified in sdf, using default value of " + gzwarn << "No 'attach_distance' specified in sdf, using default value of " << this->defaultAttachDistance << " meters.\n"; } else @@ -187,8 +187,9 @@ void DynamicDetachableJoint::PreUpdate( // if child model is not found if (kNullEntity == modelEntity) { - gzwarn << "Child Model " << this->childModelName - << " could not be found.\n"; + gzerr << "Attach Failed. child model [" << this->childModelName + << "] could not be found.\n"; + this->attachRequested = false; // reset attach request return; } @@ -200,8 +201,9 @@ void DynamicDetachableJoint::PreUpdate( // if child link is not found if (kNullEntity == this->childLinkEntity) { - gzwarn << "Child Link " << this->childLinkName - << " could not be found.\n"; + gzerr << "Attach Failed. child link [" << this->childLinkName + << "] could not be found.\n"; + this->attachRequested = false; // reset attach request return; } @@ -215,15 +217,16 @@ void DynamicDetachableJoint::PreUpdate( // Check if the child link is within the attach distance if (dist > this->attachDistance) { - gzwarn << "Child Link [" << this->childLinkName + gzerr << "Attach Failed. Child Link [" << this->childLinkName << "] is too far from parent. Distance: " << dist << "m, threshold: " << this->attachDistance << "m" << std::endl; this->attachRequested = false; // reset attach request return; } // If the child link is within the attach distance, proceed to attach - gzdbg << "Child Link " << this->childLinkName - << " is within attach distance of Parent Link. Proceeding to attach." << std::endl; + gzmsg << "Attach Success. Child model [" << this->childModelName + << "] link [" << this->childLinkName << "] attached to parent link. " + << "Distance: " << dist << "m" << std::endl; // Attach the models // We do this by creating a detachable joint entity. @@ -250,6 +253,8 @@ void DynamicDetachableJoint::PreUpdate( if (this->detachRequested && (kNullEntity != this->detachableJointEntity)) { // Detach the models + gzmsg << "Detach Success. Child model [" << this->attachedChildModelName + << "] link [" << this->attachedChildLinkName << "] detached from parent link." << std::endl; gzdbg << "Removing entity: " << this->detachableJointEntity << std::endl; _ecm.RequestRemoveEntity(this->detachableJointEntity); this->detachableJointEntity = kNullEntity; @@ -291,10 +296,10 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques if (this->isAttached) { _res.set_success(false); - _res.set_message("Already attached to child model " + this->attachedChildModelName + - " at link " + this->attachedChildLinkName + "."); - gzdbg << "Already attached to child model " << this->attachedChildModelName - << " at link " << this->attachedChildLinkName << std::endl; + _res.set_message("Already attached to child model [" + this->attachedChildModelName + + "] at link [" + this->attachedChildLinkName + "]."); + gzdbg << "Already attached to child model [" << this->attachedChildModelName + << "] at link [" << this->attachedChildLinkName << "]" << std::endl; return true; } @@ -303,8 +308,8 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques this->childLinkName = _req.child_link_name(); this->attachRequested = true; _res.set_success(true); - _res.set_message("Attached to child model " + this->childModelName + - " at link " + this->childLinkName + "."); + _res.set_message("Attach request accepted for child model [" + this->childModelName + + "] at link [" + this->childLinkName + "]."); } // If detach is requested @@ -312,11 +317,11 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques { if (!this->isAttached) { - _res.set_success(false); - _res.set_message(std::string("Detach request received for ") - + this->attachedChildModelName + "/" + this->attachedChildLinkName); - gzdbg << "Already detached" << std::endl; - return true; + _res.set_success(false); + _res.set_message(std::string("Detach request received for ") + + this->attachedChildModelName + "/" + this->attachedChildLinkName); + gzdbg << "Already detached" << std::endl; + return true; } // Validate that the request matches what is actually attached. @@ -327,17 +332,20 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques { _res.set_success(false); _res.set_message( - "Detach rejected: requested " + reqModel + "/" + reqLink + - " but currently attached to " + this->attachedChildModelName + "/" + - this->attachedChildLinkName + "." + "Detach rejected: requested [" + reqModel + "] link [" + reqLink + + "] but currently attached to [" + this->attachedChildModelName + "] link [" + + this->attachedChildLinkName + "]." ); + gzerr << "Detach rejected: requested [" << reqModel << "] link [" << reqLink + << "] but currently attached to [" << this->attachedChildModelName << "] link [" + << this->attachedChildLinkName << "]." << std::endl; return true; } this->detachRequested = true; _res.set_success(true); - _res.set_message("Detached from child model " + this->attachedChildModelName + - " at link " + this->attachedChildLinkName + "."); + _res.set_message("Detach request accepted for child model [" + this->attachedChildModelName + + "] at link [" + this->attachedChildLinkName + "]."); } else From 97fe177b36e259dde9798d57fc8b06f4632478e6 Mon Sep 17 00:00:00 2001 From: Adarsh Karan Kesavadas Prasanth Date: Thu, 2 Oct 2025 13:26:35 +0530 Subject: [PATCH 13/13] Added example world, integration test and test worlds, also verified tests Signed-off-by: Adarsh Karan Kesavadas Prasanth --- examples/worlds/dynamic_detachable_joint.sdf | 518 ++++++++++++++++ test/integration/CMakeLists.txt | 1 + test/integration/dynamic_detachable_joint.cc | 585 +++++++++++++++++++ test/worlds/dynamic_detachable_joint.sdf | 164 ++++++ 4 files changed, 1268 insertions(+) create mode 100644 examples/worlds/dynamic_detachable_joint.sdf create mode 100644 test/integration/dynamic_detachable_joint.cc create mode 100644 test/worlds/dynamic_detachable_joint.sdf diff --git a/examples/worlds/dynamic_detachable_joint.sdf b/examples/worlds/dynamic_detachable_joint.sdf new file mode 100644 index 0000000000..58cc5ebcc8 --- /dev/null +++ b/examples/worlds/dynamic_detachable_joint.sdf @@ -0,0 +1,518 @@ + + + + + + + + + + + + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.031 0.043 0.1 1 + 0.031 0.043 0.1 1 + 0.031 0.043 0.1 1 + + + + + + + + 0.5 0 0.75 0 0 0 + false + + + 40 + + + 0 0 -0.70 0 0 0 + + + 0.3 + 0.10 + + + + + 0 0 -0.70 0 0 0 + + + 0.3 + 0.10 + + + + 0.15 0.15 0.15 1 + 0.15 0.15 0.15 1 + + + + 0 0 -0.075 0 0 0 + + + 0.1 0.1 1.35 + + + + + 0 0 -0.075 0 0 0 + + + 0.1 0.1 1.35 + + + + 0.15 0.15 0.15 1 + 0.15 0.15 0.15 1 + + + + + + 0.35 0 0.25 0 0 0 + + + + 0.7 0.05 0.05 + + + + + + + 0.7 0.05 0.05 + + + + 0.15 0.15 0.15 1 + 0.15 0.15 0.15 1 + + + + + base_link + arm + + + + cmd_vel + + + + arm + /payload/attach_detach + /payload/child_state + 0.3 + + + + + + 1.4 0 0 0 0 0 + false + + + 0 0 0.375 0 0 0 + + 20 + + + + + 0.25 + 0.75 + + + + + + + 0.25 + 0.75 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0.775 0 0 0 + + 5 + + + + + 0.5 + 0.05 + + + + + + + 0.5 + 0.05 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + base + turntable + 0 0 0.75 0 0 0 + + 0 0 1 + + -1e16 + 1e16 + + + + + turntable_joint + 0.4 + + + + + + 1.400 0.350 0.85 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0.05 0.05 0.05 1 + 0.05 0.05 0.05 1 + + + + + + 1.733 0.108 0.85 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0 0 1 1 + 0 0 1 1 + + + + + + 1.606 -0.283 0.85 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0 1 0 1 + 0 1 0 1 + + + + + + 1.194 -0.283 0.85 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 1 1 0 1 + 1 1 0 1 + + + + + + 1.067 0.108 0.85 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 1 0 0 1 + 1 0 0 1 + + + + + + + + -0.2 0 0 0 0 0 + true + + + 0.30 0 0.35 0 0 0 + + + 0.02 0.6 0.7 + + + + + -0.30 0 0.35 0 0 0 + + + 0.02 0.6 0.7 + + + + + 0 0.30 0.35 0 0 0 + + + 0.6 0.02 0.7 + + + + + 0 -0.30 0.35 0 0 0 + + + 0.6 0.02 0.7 + + + + + + 0.30 0 0.35 0 0 0 + + + 0.02 0.6 0.7 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.30 0 0.35 0 0 0 + + + 0.02 0.6 0.7 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0.30 0.35 0 0 0 + + + 0.6 0.02 0.7 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 -0.30 0.35 0 0 0 + + + 0.6 0.02 0.7 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2eb075520e..858f1ca55f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -20,6 +20,7 @@ set(tests detachable_joint.cc diff_drive_system.cc drive_to_pose_controller_system.cc + dynamic_detachable_joint.cc each_new_removed.cc entity_erase.cc entity_system.cc diff --git a/test/integration/dynamic_detachable_joint.cc b/test/integration/dynamic_detachable_joint.cc new file mode 100644 index 0000000000..11185718ce --- /dev/null +++ b/test/integration/dynamic_detachable_joint.cc @@ -0,0 +1,585 @@ +/* + * Copyright (C) 2019 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * Author: Adarsh Karan K P, Neobotix GmbH + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "gz/sim/Server.hh" +#include "test_config.hh" + +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Pose.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +/// \brief Test DynamicDetachableJoint system +class DynamicDetachableJointTest : public InternalFixture<::testing::Test> +{ + public: void StartServer(const std::string &_sdfFile) + { + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + _sdfFile); + this->server = std::make_unique(serverConfig); + + EXPECT_FALSE(this->server->Running()); + EXPECT_FALSE(*this->server->Running(0)); + } + + public: std::unique_ptr server; +}; + +// See https://github.com/gazebosim/gz-sim/issues/1175 for GZ_UTILS_TEST_DISABLED_ON_WIN32(AttachDetach) + +///////////////////////////////////////////////// +// TEST 1 +// "AttachDetach" +// This test checks the following: +// 1. attach when within threshold → child (red_cube) follows parent, meaning when the rod moves up (+Z axis) the child moves along with it +// 2. detach → child free-falls due to gravity +TEST_F(DynamicDetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AttachDetach)) +{ + using namespace std::chrono_literals; + + // load the test world + this->StartServer("/test/worlds/dynamic_detachable_joint.sdf"); + + // A lambda that takes a model name and a mutable reference to a vector of + // poses and returns another lambda that can be passed to + // `Relay::OnPostUpdate`. + auto poseRecorder = + [](const std::string &_modelName, std::vector &_poses) + { + return [&, _modelName](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, const components::Model *, + const components::Name *_name, + const components::Pose *_pose) -> bool + { + if (_name->Data() == _modelName) + { + EXPECT_NE(kNullEntity, _entity); + _poses.push_back(_pose->Data()); + } + return true; + }); + }; + }; + + // the arm (aka "rod") is the parent, the cube is the child + std::vector rodPoses, cubePoses; + test::Relay rodRelay, cubeRelay; + rodRelay.OnPostUpdate(poseRecorder("rod", rodPoses)); + cubeRelay.OnPostUpdate(poseRecorder("red_cube", cubePoses)); + + this->server->AddSystem(rodRelay.systemPtr); + this->server->AddSystem(cubeRelay.systemPtr); + + // Short wait time to ensure everything is initialized + const std::size_t waitIters{20}; + this->server->Run(true, waitIters, false); + ASSERT_EQ(waitIters, cubePoses.size()); + ASSERT_EQ(waitIters, rodPoses.size()); + gzdbg << "[startup_wait] rod z=" << rodPoses.back().Pos().Z() + << ", cube z=" << cubePoses.back().Pos().Z() << std::endl; + + transport::Node node; + + // "attach" red_cube(which is on the ground) to the "rod" using the DynamicDetachableJoint service + { + msgs::AttachDetachRequest req; + req.set_child_model_name("red_cube"); + req.set_child_link_name("link"); + req.set_command("attach"); + gzdbg << "[attach] request: model=red_cube link=link" << std::endl; + + msgs::AttachDetachResponse rep; + bool result_1 = false; + bool outcome_1 = node.Request("/payload/attach_detach", + req, 1000u, rep, result_1); + + // verify service call succeeded + ASSERT_TRUE(outcome_1); + ASSERT_TRUE(result_1); + ASSERT_TRUE(rep.success()) << rep.message(); + gzdbg << "[attach] ok" << std::endl; + } + + // Wait + { + // Reset poses + cubePoses.clear(); + rodPoses.clear(); + + const std::size_t iters{60}; + this->server->Run(true, iters, false); + + // Verify if we recorded expected number of poses + ASSERT_EQ(iters, cubePoses.size()); + ASSERT_EQ(iters, rodPoses.size()); + + // Additional check to ensure the red_cube is rigidly attached to the rod by comparing relative pose + auto rel0 = cubePoses.front().Pos() - rodPoses.front().Pos(); + auto rel1 = cubePoses.back().Pos() - rodPoses.back().Pos(); + EXPECT_LT((rel1 - rel0).Length(), 5e-4); + gzdbg << "[attached-idle] rel Δ=" << (rel1 - rel0).Length() + << " (expect ~0)" << std::endl; + } + + // "lift" the rod up in Z so the attached red_cube is above ground + { + gzdbg << "[lift] moving rod from z 0.14m to 0.35m" << std::endl; + msgs::Pose poseReq; + poseReq.set_name("rod"); + // keep x,y (same as sdf) and update only z + auto *pos = poseReq.mutable_position(); + pos->set_x(0.30); pos->set_y(0.0); pos->set_z(0.35); + auto *ori = poseReq.mutable_orientation(); + ori->set_x(0); ori->set_y(0); ori->set_z(0); ori->set_w(1); + + msgs::Boolean rep; + bool result_2 = false; + bool outcome_2 = node.Request("/world/dynamic_detachable_joint_test/set_pose", + poseReq, 1000u, rep, result_2); + + // verify service call succeeded + ASSERT_TRUE(outcome_2); + ASSERT_TRUE(result_2); + ASSERT_TRUE(rep.data()); + gzdbg << "[lift] pose service ok" << std::endl; + } + + // After lifting, child follows upward along with the parent rod + // this causes it's Z position to change + double pos_before_lift = cubePoses.back().Pos().Z(); + { + cubePoses.clear(); + rodPoses.clear(); + const std::size_t iters{50}; + this->server->Run(true, iters, false); + ASSERT_EQ(iters, cubePoses.size()); + ASSERT_EQ(iters, rodPoses.size()); + + // Track how much the red_cube moved up + auto movedZ = cubePoses.back().Pos().Z() - pos_before_lift; + EXPECT_GT(movedZ, 0.15); // followed rod upwards + gzdbg << "[after-lift] red_cube Δz=" << movedZ << " (expect ~0.21)" << std::endl; + + // Additional check to ensure the red_cube is rigidly attached to the rod by comparing relative pose + auto rel0 = cubePoses.front().Pos() - rodPoses.front().Pos(); + auto rel1 = cubePoses.back().Pos() - rodPoses.back().Pos(); + EXPECT_LT((rel1 - rel0).Length(), 5e-4); + } + + // "detach" and verify free fall of the red_cube + { + gzdbg << "[detach] request: model=red_cube link=link" << std::endl; + msgs::AttachDetachRequest req; + req.set_child_model_name("red_cube"); + req.set_child_link_name("link"); + req.set_command("detach"); + + msgs::AttachDetachResponse rep; + bool result_3 = false; + bool outcome_3 = node.Request("/payload/attach_detach", + req, 1000u, rep, result_3); + + // verify service call succeeded + ASSERT_TRUE(outcome_3); + ASSERT_TRUE(result_3); + ASSERT_TRUE(rep.success()) << rep.message(); + gzdbg << "[detach] ok" << std::endl; + } + + // The ideal distance an object falls under gravity is given as d = 0.5 * g * t^2 + // Let it fall for about 150 ms, then check if dz (z distance travelled) > d + { + cubePoses.clear(); + const std::size_t iters{150}; + this->server->Run(true, iters, false); + ASSERT_EQ(iters, cubePoses.size()); + + const double dt = 0.001; // 1 ms from the test world + const double t = static_cast(iters - 1) * dt; // N poses give N-1 integration intervals + const double d = 0.5 * 9.8 * t * t; + const double dz = cubePoses.front().Pos().Z() - cubePoses.back().Pos().Z(); + + // Check if the red_cube fell "at least" the ideal distance + EXPECT_GT(dz, d); + gzdbg << "[free-fall] dz=" << dz << " > " << d + << " (t=" << t << "s, N=" << iters << ")" << std::endl; + } +} + +///////////////////////////////////////////////// +// TEST 2 +// "OutOfRange" +// This test checks the following: +// 1. move the parent (rod) far enough so that the center-to-center distance +// to the red_cube exceeds the configured attach_distance (0.1 m) +// 2. call "attach" → service accepts the request (rep.success() == true); +// 3. verify the target child "red_cube" stayed on the ground (~z = 0.05) +TEST_F(DynamicDetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OutOfRange)) +{ + using namespace std::chrono_literals; + + this->StartServer("/test/worlds/dynamic_detachable_joint.sdf"); + + // A lambda that records poses for a named model + auto poseRecorder = + [](const std::string &_name, std::vector &_poses) + { + return [&, _name](const UpdateInfo &, const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, const components::Model *, + const components::Name *n, const components::Pose *p) -> bool + { + if (n->Data() == _name) _poses.push_back(p->Data()); + return true; + }); + }; + }; + + std::vector rodPoses, cubePoses; + test::Relay rodRelay, cubeRelay; + rodRelay.OnPostUpdate(poseRecorder("rod", rodPoses)); + cubeRelay.OnPostUpdate(poseRecorder("red_cube", cubePoses)); + + this->server->AddSystem(rodRelay.systemPtr); + this->server->AddSystem(cubeRelay.systemPtr); + + // Short wait time to ensure everything is initialized + const std::size_t waitIters{20}; + this->server->Run(true, waitIters, false); + ASSERT_EQ(waitIters, rodPoses.size()); + ASSERT_EQ(waitIters, cubePoses.size()); + gzdbg << "[startup_wait] rod z=" << rodPoses.back().Pos().Z() + << ", red_cube z=" << cubePoses.back().Pos().Z() << std::endl; + + // Move rod high enough so center distance > 0.1 m (attach_distance). + // red_cube remains at center z = 0.05m and set rod z to 0.50m → gap = 0.45 m. + transport::Node node; + { + msgs::Pose poseReq; + poseReq.set_name("rod"); + auto *pos = poseReq.mutable_position(); + pos->set_x(0.30); pos->set_y(0.0); pos->set_z(0.50); + auto *ori = poseReq.mutable_orientation(); + ori->set_x(0); ori->set_y(0); ori->set_z(0); ori->set_w(1); + + msgs::Boolean rep; + bool result_1 = false; + bool outcome_1 = node.Request("/world/dynamic_detachable_joint_test/set_pose", + poseReq, 1000u, rep, result_1); + + // verify service call succeeded + ASSERT_TRUE(outcome_1); + ASSERT_TRUE(result_1); + ASSERT_TRUE(rep.data()); + gzdbg << "[lift-far] pose service ok (rod z=0.50)" << std::endl; + } + // Let the pose update apply before attempting attach + this->server->Run(true, 5, false); + + // Additional check to ensure if gap is actually larger than attach_distance (0.1 m) + { + ASSERT_FALSE(rodPoses.empty()); + ASSERT_FALSE(cubePoses.empty()); + const double gap = std::abs(rodPoses.back().Pos().Z() - cubePoses.back().Pos().Z()); + ASSERT_GT(gap, 0.1 + 1e-3); + } + + // Attempt to "attach" → request is processed, but attach is rejected in PreUpdate of the plugin + // Error is logged if out of range, and we verify by poses that nothing attached + { + msgs::AttachDetachRequest req; + req.set_child_model_name("red_cube"); + req.set_child_link_name("link"); + req.set_command("attach"); + + msgs::AttachDetachResponse rep; + bool result_2 = false; + bool outcome_2 = node.Request("/payload/attach_detach", + req, 1000u, rep, result_2); + + // verify service call succeeded + ASSERT_TRUE(outcome_2); + ASSERT_TRUE(result_2); + gzdbg << "[attach-out-of-range] request processed and an error is logged if out of range" + << std::endl; + } + + // Additional check to ensure red_cube is still on the ground + // red_cube should still be on the ground (~0.05) + { + cubePoses.clear(); + const std::size_t iters{40}; + this->server->Run(true, iters, false); + ASSERT_EQ(iters, cubePoses.size()); + EXPECT_NEAR(cubePoses.back().Pos().Z(), 0.05, 1e-2); + gzdbg << "[verify-ground] red_cube z=" << cubePoses.back().Pos().Z() + << " (expect ~0.05)" << std::endl; + } +} + +///////////////////////////////////////////////// +// TEST 3 +// "MultipleChildrenSequential" +// This test checks the following +// 1. attach lift detach each target child in turn (red_cube, green_cube, blue_cube) +// 2. only the attached child follows the parent rod when it moves up (+Z axis) +// 3. after detaching the target child free-falls due to gravity +// 4. non-target children stay on the ground (~z = 0.05) +TEST_F(DynamicDetachableJointTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(MultipleChildrenSequential)) +{ + using namespace std::chrono_literals; + + this->StartServer("/test/worlds/dynamic_detachable_joint.sdf"); + + // Record poses for rod and all 3 cubes + auto poseRecorder = + [](const std::string &_name, std::vector &_poses) + { + return [&, _name](const UpdateInfo &, const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, const components::Model *, + const components::Name *n, const components::Pose *p) -> bool + { + if (n->Data() == _name) _poses.push_back(p->Data()); + return true; + }); + }; + }; + + std::vector rodPoses, redPoses, greenPoses, bluePoses; + test::Relay rodRelay, redRelay, greenRelay, blueRelay; + rodRelay.OnPostUpdate(poseRecorder("rod", rodPoses)); + redRelay.OnPostUpdate(poseRecorder("red_cube", redPoses)); + greenRelay.OnPostUpdate(poseRecorder("green_cube", greenPoses)); + blueRelay.OnPostUpdate(poseRecorder("blue_cube", bluePoses)); + + this->server->AddSystem(rodRelay.systemPtr); + this->server->AddSystem(redRelay.systemPtr); + this->server->AddSystem(greenRelay.systemPtr); + this->server->AddSystem(blueRelay.systemPtr); + + // wait to ensure everything is initialized + const std::size_t waitIters{20}; + this->server->Run(true, waitIters, false); + ASSERT_EQ(waitIters, rodPoses.size()); + ASSERT_EQ(waitIters, redPoses.size()); + ASSERT_EQ(waitIters, greenPoses.size()); + ASSERT_EQ(waitIters, bluePoses.size()); + + transport::Node node; + + struct Target { const char *name; double x; std::vector* poses; }; + std::array targets{{ + {"red_cube", 0.30, &redPoses}, + {"green_cube", 0.10, &greenPoses}, + {"blue_cube", 0.50, &bluePoses}, + }}; + + auto setRodPose = [&](double x, double z) + { + msgs::Pose poseReq; + poseReq.set_name("rod"); + auto *pos = poseReq.mutable_position(); + pos->set_x(x); pos->set_y(0.0); pos->set_z(z); + auto *ori = poseReq.mutable_orientation(); + ori->set_x(0); ori->set_y(0); ori->set_z(0); ori->set_w(1); + + msgs::Boolean rep; + bool result_pose = false; + bool outcome_pose = node.Request("/world/dynamic_detachable_joint_test/set_pose", + poseReq, 1000u, rep, result_pose); + + // verify service call succeeded + ASSERT_TRUE(outcome_pose); + ASSERT_TRUE(result_pose); + ASSERT_TRUE(rep.data()); + }; + + auto nearGround = [](double z){ return std::abs(z - 0.05) < 1e-2; }; + + for (const auto &target : targets) + { + // Move rod above target within attach_distance (0.1 m) + setRodPose(target.x, 0.14); + this->server->Run(true, 5, false); + + // "attach" target child to the rod using the service + { + gzdbg << "[attach] request: model=" << target.name << " link=link" << std::endl; + + msgs::AttachDetachRequest req; + req.set_child_model_name(target.name); + req.set_child_link_name("link"); + req.set_command("attach"); + + msgs::AttachDetachResponse rep; + bool result_attach = false; + bool outcome_attach = node.Request("/payload/attach_detach", + req, 1000u, rep, result_attach); + + ASSERT_TRUE(outcome_attach); + ASSERT_TRUE(result_attach); + ASSERT_TRUE(rep.success()) << rep.message(); + gzdbg << "[attach] ok target=" << target.name << std::endl; + } + + // Step and verify + // 1. the child is rigidly attached to parent + // 2. other target cubes stay on ground + { + rodPoses.clear(); redPoses.clear(); greenPoses.clear(); bluePoses.clear(); + const std::size_t iters{30}; + this->server->Run(true, iters, false); + + ASSERT_EQ(iters, rodPoses.size()); + ASSERT_EQ(iters, redPoses.size()); + ASSERT_EQ(iters, greenPoses.size()); + ASSERT_EQ(iters, bluePoses.size()); + + const auto &v = *target.poses; + auto rel0 = v.front().Pos() - rodPoses.front().Pos(); + auto rel1 = v.back().Pos() - rodPoses.back().Pos(); + EXPECT_LT((rel1 - rel0).Length(), 5e-4); + + if (target.name != std::string("red_cube")) + { + EXPECT_TRUE(nearGround(redPoses.back().Pos().Z())); + } + if (target.name != std::string("green_cube")) + { + EXPECT_TRUE(nearGround(greenPoses.back().Pos().Z())); + } + if (target.name != std::string("blue_cube")) + { + EXPECT_TRUE(nearGround(bluePoses.back().Pos().Z())); + } + } + + // "lift" the rod to z 0.35 and verify only target follows upward + double pos_before_lift = target.poses->back().Pos().Z(); + setRodPose(target.x, 0.35); + { + rodPoses.clear(); redPoses.clear(); greenPoses.clear(); bluePoses.clear(); + const std::size_t iters{50}; + this->server->Run(true, iters, false); + + ASSERT_EQ(iters, rodPoses.size()); + ASSERT_EQ(iters, redPoses.size()); + ASSERT_EQ(iters, greenPoses.size()); + ASSERT_EQ(iters, bluePoses.size()); + + double movedZ = target.poses->back().Pos().Z() - pos_before_lift; + EXPECT_GT(movedZ, 0.15); + + if (target.name != std::string("red_cube")) + { + EXPECT_TRUE(nearGround(redPoses.back().Pos().Z())); + } + if (target.name != std::string("green_cube")) + { + EXPECT_TRUE(nearGround(greenPoses.back().Pos().Z())); + } + if (target.name != std::string("blue_cube")) + { + EXPECT_TRUE(nearGround(bluePoses.back().Pos().Z())); + } + } + + // "detach" the target and verify free-fall + { + gzdbg << "[detach] request: model=" << target.name << " link=link" << std::endl; + + msgs::AttachDetachRequest req; + req.set_child_model_name(target.name); + req.set_child_link_name("link"); + req.set_command("detach"); + + msgs::AttachDetachResponse rep; + bool result_detach = false; + bool outcome_detach = node.Request("/payload/attach_detach", + req, 1000u, rep, result_detach); + + ASSERT_TRUE(outcome_detach); + ASSERT_TRUE(result_detach); + ASSERT_TRUE(rep.success()) << rep.message(); + gzdbg << "[detach] ok target=" << target.name << std::endl; + } + { + rodPoses.clear(); redPoses.clear(); greenPoses.clear(); bluePoses.clear(); + const std::size_t iters{150}; + this->server->Run(true, iters, false); + + ASSERT_EQ(iters, redPoses.size()); + ASSERT_EQ(iters, greenPoses.size()); + ASSERT_EQ(iters, bluePoses.size()); + + const double dt = 0.001; // 1 ms from the test world + const double t = static_cast(iters - 1) * dt; // N poses give N-1 integration intervals + const double d = 0.5 * 9.8 * t * t; + const auto &v = *target.poses; + const double dz = v.front().Pos().Z() - v.back().Pos().Z(); + EXPECT_GT(dz, d); + + if (target.name != std::string("red_cube")) + { + EXPECT_TRUE(nearGround(redPoses.back().Pos().Z())); + } + if (target.name != std::string("green_cube")) + { + EXPECT_TRUE(nearGround(greenPoses.back().Pos().Z())); + } + if (target.name != std::string("blue_cube")) + { + EXPECT_TRUE(nearGround(bluePoses.back().Pos().Z())); + } + } + + // Let the detached target land then reset rod for next target + { + const std::size_t landIters{350}; + this->server->Run(true, landIters, false); + EXPECT_NEAR(target.poses->back().Pos().Z(), 0.05, 1e-2); + } + + setRodPose(target.x, 0.14); + this->server->Run(true, 5, false); + } +} diff --git a/test/worlds/dynamic_detachable_joint.sdf b/test/worlds/dynamic_detachable_joint.sdf new file mode 100644 index 0000000000..c1217fa4c3 --- /dev/null +++ b/test/worlds/dynamic_detachable_joint.sdf @@ -0,0 +1,164 @@ + + + + + + + + 0.001 + 0 + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + + + 0.30 0 0.14 0 0 0 + + false + + 1.0 + + + + + 0.6 0.05 0.05 + + + + + + + 0.6 0.05 0.05 + + + + 0.15 0.15 0.15 1 + 0.15 0.15 0.15 1 + + + + + + arm + /payload/attach_detach + /payload/child_state + 0.1 + + + + + + 0.10 0 0.05 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0 1 0 1 + 0 1 0 1 + + + + + + + 0.30 0 0.05 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 1 0 0 1 + 1 0 0 1 + + + + + + + 0.50 0 0.05 0 0 0 + + + 0.2 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + 0 0 1 1 + 0 0 1 1 + + + + + +