Merge 5bf2a93 into e37ff8f

examples/worlds/detachable_joint.sdf

@@ -13,6 +13,18 @@
       ign topic -t "/B1/detach" -m ignition.msgs.Empty -p "unused: true"
       ign topic -t "/B2/detach" -m ignition.msgs.Empty -p "unused: true"
       ign topic -t "/B3/detach" -m ignition.msgs.Empty -p "unused: true"
+
+    To re-attach breadcrumbs
+
+      ign topic -t "/B1/attach" -m ignition.msgs.Empty -p "unused: true"
+      ign topic -t "/B2/attach" -m ignition.msgs.Empty -p "unused: true"
+      ign topic -t "/B3/attach" -m ignition.msgs.Empty -p "unused: true"
+
+    To monitor the state of each breadcrumbs
+
+      ign topic -e -t /B1/state
+      ign topic -e -t /B2/state
+      ign topic -e -t /B3/state
 -->
 
 <sdf version="1.6">
@@ -353,23 +365,32 @@
         <wheel_separation>1.25</wheel_separation>
         <wheel_radius>0.3</wheel_radius>
       </plugin>
-      <plugin filename="ignition-gazebo-detachable-joint-system" name="gz::sim::systems::DetachableJoint">
+      <plugin filename="ignition-gazebo-detachable-joint-system"
+              name="ignition::gazebo::systems::DetachableJoint">
        <parent_link>chassis</parent_link>
        <child_model>B1</child_model>
        <child_link>body</child_link>
-       <topic>/B1/detach</topic>
+       <detach_topic>/B1/detach</detach_topic>
+       <attach_topic>/B1/attach</attach_topic>
+       <output_topic>/B1/state</output_topic>
       </plugin>
-      <plugin filename="ignition-gazebo-detachable-joint-system" name="gz::sim::systems::DetachableJoint">
+      <plugin filename="ignition-gazebo-detachable-joint-system"
+              name="ignition::gazebo::systems::DetachableJoint">
        <parent_link>chassis</parent_link>
        <child_model>B2</child_model>
        <child_link>body</child_link>
-       <topic>/B2/detach</topic>
+       <detach_topic>/B2/detach</detach_topic>
+       <attach_topic>/B2/attach</attach_topic>
+       <output_topic>/B2/state</output_topic>
       </plugin>
-      <plugin filename="ignition-gazebo-detachable-joint-system" name="gz::sim::systems::DetachableJoint">
+      <plugin filename="ignition-gazebo-detachable-joint-system"
+              name="ignition::gazebo::systems::DetachableJoint">
        <parent_link>chassis</parent_link>
        <child_model>B3</child_model>
        <child_link>body</child_link>
-       <topic>/B3/detach</topic>
+       <detach_topic>/B3/detach</detach_topic>
+       <attach_topic>/B3/attach</attach_topic>
+       <output_topic>/B3/state</output_topic>
       </plugin>
     </model>
 

examples/worlds/triggered_publisher.sdf

@@ -391,13 +391,13 @@ start falling.
         <parent_link>body</parent_link>
         <child_model>box1</child_model>
         <child_link>box_body</child_link>
-        <topic>/box1/detach</topic>
+        <detach_topic>/box1/detach</detach_topic>
       </plugin>
       <plugin filename="ignition-gazebo-detachable-joint-system" name="gz::sim::systems::DetachableJoint">
         <parent_link>body</parent_link>
         <child_model>box2</child_model>
         <child_link>box_body</child_link>
-        <topic>/box2/detach</topic>
+        <detach_topic>/box2/detach</detach_topic>
       </plugin>
     </model>
 

src/systems/detachable_joint/DetachableJoint.cc

@@ -96,15 +96,115 @@ void DetachableJoint::Configure(const Entity &_entity,
   }
 
   // Setup detach topic
-  std::vector<std::string> topics;
-  if (_sdf->HasElement("topic"))
+  std::vector<std::string> detachTopics;
+  if (_sdf->HasElement("detach_topic"))
   {
-    topics.push_back(_sdf->Get<std::string>("topic"));
+    detachTopics.push_back(_sdf->Get<std::string>("detach_topic"));
   }
-  topics.push_back("/model/" + this->model.Name(_ecm) +
+  detachTopics.push_back("/model/" + this->model.Name(_ecm) +
       "/detachable_joint/detach");
-  this->topic = validTopic(topics);
 
+  if (_sdf->HasElement("topic"))
+  {
+    if (_sdf->HasElement("detach_topic"))
+    {
+      if (_sdf->Get<std::string>("topic") !=
+          _sdf->Get<std::string>("detach_topic"))
+      {
+        ignerr << "<topic> and <detach_topic> tags have different contents. "
+                  "Please verify the correct string and use <detach_topic>."
+               << std::endl;
+      }
+      else
+      {
+        igndbg << "Ignoring <topic> tag and using <detach_topic> tag."
+                << std::endl;
+      }
+    }
+    else
+    {
+      detachTopics.insert(detachTopics.begin(),
+                          _sdf->Get<std::string>("topic"));
+    }
+  }
+
+  this->detachTopic = validTopic(detachTopics);
+  if (this->detachTopic.empty())
+  {
+    ignerr << "No valid detach topics for DetachableJoint could be found.\n";
+    return;
+  }
+  igndbg << "Detach topic is: " << this->detachTopic << std::endl;
+
+  // Setup subscriber for detach topic
+  this->node.Subscribe(
+      this->detachTopic, &DetachableJoint::OnDetachRequest, this);
+
+  igndbg << "DetachableJoint subscribing to messages on "
+         << "[" << this->detachTopic << "]" << std::endl;
+
+  // Setup attach topic
+  std::vector<std::string> attachTopics;
+  if (_sdf->HasElement("attach_topic"))
+  {
+    attachTopics.push_back(_sdf->Get<std::string>("attach_topic"));
+  }
+  attachTopics.push_back("/model/" + this->model.Name(_ecm) +
+      "/detachable_joint/attach");
+  this->attachTopic = validTopic(attachTopics);
+  if (this->attachTopic.empty())
+  {
+    ignerr << "No valid attach topics for DetachableJoint could be found.\n";
+    return;
+  }
+  igndbg << "Attach topic is: " << this->attachTopic << std::endl;
+
+  // Setup subscriber for attach topic
+  auto msgCb = std::function<void(const transport::ProtoMsg &)>(
+      [this](const auto &)
+      {
+        if (this->isAttached){
+          igndbg << "Already attached" << std::endl;
+          return;
+        }
+        this->attachRequested = true;
+      });
+
+  if (!this->node.Subscribe(this->attachTopic, msgCb))
+  {
+    ignerr << "Subscriber could not be created for [attach] topic.\n";
+    return;
+  }
+
+  // Setup output topic
+  std::vector<std::string> outputTopics;
+  if (_sdf->HasElement("output_topic"))
+  {
+    outputTopics.push_back(_sdf->Get<std::string>("output_topic"));
+  }
+
+  outputTopics.push_back("/model/" + this->childModelName +
+      "/detachable_joint/state");
+
+  this->outputTopic = validTopic(outputTopics);
+  if (this->outputTopic.empty())
+  {
+    ignerr << "No valid output topics for DetachableJoint could be found.\n";
+    return;
+  }
+  igndbg << "Output topic is: " << this->outputTopic << std::endl;
+
+  // Setup publisher for output topic
+  this->outputPub = this->node.Advertise<ignition::msgs::StringMsg>(
+      this->outputTopic);
+  if (!this->outputPub)
+  {
+    ignerr << "Error advertising topic [" << this->outputTopic << "]"
+              << std::endl;
+    return;
+  }
+
+  // Supress Child Warning
   this->suppressChildWarning =
       _sdf->Get<bool>("suppress_child_warning", this->suppressChildWarning)
           .first;
@@ -118,8 +218,13 @@ void DetachableJoint::PreUpdate(
   EntityComponentManager &_ecm)
 {
   IGN_PROFILE("DetachableJoint::PreUpdate");
-  if (this->validConfig && !this->initialized)
+  // 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};
 
@@ -148,14 +253,11 @@ void DetachableJoint::PreUpdate(
             this->detachableJointEntity,
             components::DetachableJoint({this->parentLinkEntity,
                                          this->childLinkEntity, "fixed"}));
-
-        this->node.Subscribe(
-            this->topic, &DetachableJoint::OnDetachRequest, this);
-
-        ignmsg << "DetachableJoint subscribing to messages on "
-               << "[" << this->topic << "]" << std::endl;
-
-        this->initialized = true;
+        this->attachRequested = false;
+        this->isAttached = true;
+        this->PublishJointState(this->isAttached);
+        igndbg << "Attaching entity: " << this->detachableJointEntity
+               << std::endl;
       }
       else
       {
@@ -170,7 +272,8 @@ void DetachableJoint::PreUpdate(
     }
   }
 
-  if (this->initialized)
+ // only allow detaching if child entity is attached
+  if (this->isAttached)
   {
     if (this->detachRequested && (kNullEntity != this->detachableJointEntity))
     {
@@ -179,13 +282,34 @@ void DetachableJoint::PreUpdate(
       _ecm.RequestRemoveEntity(this->detachableJointEntity);
       this->detachableJointEntity = kNullEntity;
       this->detachRequested = false;
+      this->isAttached = false;
+      this->PublishJointState(this->isAttached);
     }
   }
 }
 
+//////////////////////////////////////////////////
+void DetachableJoint::PublishJointState(bool attached)
+{
+  ignition::msgs::StringMsg detachedStateMsg;
+  if (attached)
+  {
+    detachedStateMsg.set_data("attached");
+  }
+  else
+  {
+    detachedStateMsg.set_data("detached");
+  }
+  this->outputPub.Publish(detachedStateMsg);
+}
+
 //////////////////////////////////////////////////
 void DetachableJoint::OnDetachRequest(const msgs::Empty &)
 {
+  if (!this->isAttached){
+    igndbg << "Already detached" << std::endl;
+    return;
+  }
   this->detachRequested = true;
 }
 

src/systems/detachable_joint/DetachableJoint.hh

@@ -36,7 +36,9 @@ inline namespace IGNITION_GAZEBO_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.
+  /// 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.
   ///
   /// Parameters:
   ///
@@ -48,7 +50,21 @@ namespace systems
   /// - `<child_link>`: Name of the link in the child model to be used in
   /// creating a fixed joint with a link in the parent model.
   ///
-  /// - `<topic>` (optional): Topic name to be used for detaching connections
+  /// - `<topic>` (optional): Topic name to be used for detaching connections.
+  /// Using <detach_topic> is preferred.
+  ///
+  /// - `<detach_topic>` (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.
+  ///
+  /// - `<attach_topic>` (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.
+  ///
+  /// - `<output_topic>` (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.
   ///
   /// - `<suppress_child_warning>` (optional): If true, the system
   /// will not print a warning message if a child model does not exist yet.
@@ -73,6 +89,15 @@ namespace systems
                 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 Helper function to publish the state of the detachment
+    private: void PublishJointState(bool attached);
+
     /// \brief Callback for detach request topic
     private: void OnDetachRequest(const msgs::Empty &_msg);
 
@@ -86,7 +111,13 @@ namespace systems
     private: std::string childLinkName;
 
     /// \brief Topic to be used for detaching connections
-    private: std::string topic;
+    private: std::string detachTopic;
+
+    /// \brief Topic to be used for re-attaching connections
+    private: std::string attachTopic;
+
+    /// \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};
@@ -103,14 +134,15 @@ namespace systems
     /// \brief Whether detachment has been requested
     private: std::atomic<bool> detachRequested{false};
 
-    /// \brief Ignition communication node.
-    public: transport::Node node;
+    /// \brief Whether attachment has been requested
+    private: std::atomic<bool> attachRequested{true};
+
+    /// \brief Whether child entity is attached
+    private: std::atomic<bool> isAttached{false};
 
     /// \brief Whether all parameters are valid and the system can proceed
     private: bool validConfig{false};
 
-    /// \brief Whether the system has been initialized
-    private: bool initialized{false};
   };
   }
 }