diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index 1aea0d3a24..2ccd32e622 100644 --- a/examples/worlds/detachable_joint.sdf +++ b/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 --> @@ -353,23 +365,32 @@ 1.25 0.3 - + chassis B1 body - /B1/detach + /B1/detach + /B1/attach + /B1/state - + chassis B2 body - /B2/detach + /B2/detach + /B2/attach + /B2/state - + chassis B3 body - /B3/detach + /B3/detach + /B3/attach + /B3/state diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index dbc2a43022..4d18bc4fed 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -391,13 +391,13 @@ start falling. body box1 box_body - /box1/detach + /box1/detach body box2 box_body - /box2/detach + /box2/detach diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 15885a14f0..f41a26d93d 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -96,15 +96,115 @@ void DetachableJoint::Configure(const Entity &_entity, } // Setup detach topic - std::vector topics; - if (_sdf->HasElement("topic")) + std::vector detachTopics; + if (_sdf->HasElement("detach_topic")) { - topics.push_back(_sdf->Get("topic")); + detachTopics.push_back(_sdf->Get("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("topic") != + _sdf->Get("detach_topic")) + { + ignerr << " and tags have different contents. " + "Please verify the correct string and use ." + << std::endl; + } + else + { + igndbg << "Ignoring tag and using tag." + << std::endl; + } + } + else + { + detachTopics.insert(detachTopics.begin(), + _sdf->Get("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 attachTopics; + if (_sdf->HasElement("attach_topic")) + { + attachTopics.push_back(_sdf->Get("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( + [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 outputTopics; + if (_sdf->HasElement("output_topic")) + { + outputTopics.push_back(_sdf->Get("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( + this->outputTopic); + if (!this->outputPub) + { + ignerr << "Error advertising topic [" << this->outputTopic << "]" + << std::endl; + return; + } + + // Supress Child Warning this->suppressChildWarning = _sdf->Get("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; } diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index 57fdfca98b..f0f800b4e5 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/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 /// - ``: 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 + /// - `` (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. @@ -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 detachRequested{false}; - /// \brief Ignition communication node. - public: transport::Node node; + /// \brief Whether attachment has been requested + private: std::atomic attachRequested{true}; + + /// \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}; - /// \brief Whether the system has been initialized - private: bool initialized{false}; }; } } diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 0c3b19f9e5..f80240546b 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -213,3 +213,145 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) // the expected distance. EXPECT_GT(b2Poses.front().Pos().Z() - b2Poses.back().Pos().Z(), expDist); } + + ///////////////////////////////////////////////// + // Test for re-attaching a detached joint. This uses the vehicle_blue and B1 + // box models. The B1 model is first detached from the vehicle. Although + // detached, the distance (x-direction) between B1 and vehicle is 1.5, which + // is the default offset. Then, linear velocity of 1.0 is published on the + // `/cmd_vel` topic. After 200 iterations, the B1 model will remain in the same + // position whereas the vehicle will move in the x-direction. Now the + // distance between B1 and the vehicle will be the default offset (1.5) + // in addition to the distance traveled by the vehicle. Next, B1 is re-attached + // to the vehicle. After 200 iterations, we can confirm that B1 has moved with + // the vehicle and the distance traveled by B1 is close to that of the vehicle. + // Therefore, it confirms that B1 is re-attached to the vehicle. + TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ReAttach)) + { + using namespace std::chrono_literals; + + this->StartServer(common::joinPaths("/test", "worlds", + "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 gazebo::UpdateInfo &, + const gazebo::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; + }); + }; + }; + + std::vector b1Poses, vehiclePoses; + test::Relay testSystem1; + testSystem1.OnPostUpdate(poseRecorder("B1", b1Poses)); + test::Relay testSystem2; + testSystem2.OnPostUpdate(poseRecorder("vehicle_blue", vehiclePoses)); + + this->server->AddSystem(testSystem1.systemPtr); + this->server->AddSystem(testSystem2.systemPtr); + + transport::Node node; + // time required for the child and parent links to be attached + igndbg << "Initially attaching the links" << std::endl; + const std::size_t nItersInitialize{100}; + this->server->Run(true, nItersInitialize, false); + + // detach the B1 model from the vehicle model + auto pub = node.Advertise("/B1/detach"); + pub.Publish(msgs::Empty()); + std::this_thread::sleep_for(250ms); + const std::size_t nItersAfterDetach{100}; + this->server->Run(true, nItersAfterDetach, false); + + ASSERT_EQ(nItersAfterDetach + nItersInitialize, b1Poses.size()); + ASSERT_EQ(nItersAfterDetach + nItersInitialize, vehiclePoses.size()); + + // Deafult distance between B1 and the vehicle is 1.5. + auto defaultDist = 1.5; + // Although detached, distance (x-axis) between B1 and vehicle should be 1.5. + EXPECT_NEAR(vehiclePoses.back().Pos().X(), + abs(b1Poses.back().Pos().X()) - defaultDist, 0.0001); + + // clear the vectors + b1Poses.clear(); + vehiclePoses.clear(); + + // Move the vehicle along the x-axis with linear speed of x = 1.0. Since B1 + // has been detached, it's just the vehicle moving forward. + auto cmdVelPub = node.Advertise("/model/vehicle_blue/cmd_vel"); + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(1.0, 0, 0)); + cmdVelPub.Publish(msg); + std::this_thread::sleep_for(250ms); + const std::size_t nItersAfterMoving{200}; + this->server->Run(true, nItersAfterMoving, false); + + ASSERT_EQ(nItersAfterMoving, b1Poses.size()); + ASSERT_EQ(nItersAfterMoving, vehiclePoses.size()); + + // Model B1 X pos is stationary. Therefore the diff will be close to 0. + EXPECT_TRUE(abs(b1Poses.front().Pos().X() - + b1Poses.back().Pos().X()) < 0.001); + + // Model vehicle_blue X pos will be different since it moved. + auto distTraveled = 0.1; + EXPECT_TRUE(abs(vehiclePoses.front().Pos().X() - + vehiclePoses.back().Pos().X()) > distTraveled); + + // Distance between the B1 and vehicle model confirms that it is detached + // and the vehicle traveled away from B1. + auto totalDist = defaultDist + distTraveled; + EXPECT_TRUE(abs(vehiclePoses.back().Pos().X() - + b1Poses.back().Pos().X()) > totalDist); + + // clear the vectors + b1Poses.clear(); + vehiclePoses.clear(); + + // Now re-attach the B1 model back to the vehicle. B1 will move with the + // vehicle. + auto attachPub = node.Advertise("/B1/attach"); + attachPub.Publish(msgs::Empty()); + std::this_thread::sleep_for(250ms); + const std::size_t nItersAfterMovingTogether{200}; + this->server->Run(true, nItersAfterMovingTogether, false); + + ASSERT_EQ(nItersAfterMovingTogether, b1Poses.size()); + ASSERT_EQ(nItersAfterMovingTogether, vehiclePoses.size()); + + // Model B1 should move along with the vehicle. Therefore the position should + // change. + EXPECT_TRUE(abs(b1Poses.front().Pos().X() - + b1Poses.back().Pos().X()) > distTraveled); + + // distance traveled along the x-axis by the B1 model + auto distTraveledB1 = abs(b1Poses.back().Pos().X() - + b1Poses.front().Pos().X()); + + // distance traveled along the x-axis by the vehicle model + auto distTraveledVehicle = abs(vehiclePoses.back().Pos().X() - + vehiclePoses.front().Pos().X()); + igndbg << "dist by B1: " << distTraveledB1 << " ,dist by vehicle: " + << distTraveledVehicle << ", diff: " + << abs(distTraveledB1 - distTraveledVehicle) << std::endl; + + // since the two models are attached, the distances traveled by both objects + // should be close. + EXPECT_TRUE(abs(distTraveledB1 - distTraveledVehicle) < 0.01); + } diff --git a/test/worlds/detachable_joint.sdf b/test/worlds/detachable_joint.sdf index afe0a7efac..a9b73307aa 100644 --- a/test/worlds/detachable_joint.sdf +++ b/test/worlds/detachable_joint.sdf @@ -45,7 +45,8 @@ - + body M2 body @@ -76,7 +77,6 @@ - 10 0 1 0 0 0 @@ -120,11 +120,339 @@ - + body1 __model__ body2 + + + 0 2 0.325 0 -0 0 + true + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 1.25 + 0.3 + + + chassis + B1 + body + /B1/detach + /B1/attach + + + + + + -1.5 2.35 0.5 0 -0 0 + + + 0.6 + + 0.017 + 0 + 0 + 0.017 + 0 + 0.009 + + + + + + 0.3 0.3 0.5 + + + + 0.0 1.0 0.0 1 + 0.0 1.0 0.0 1 + 0.5 0.5 0.5 1 + + + + + + 0.3 0.3 0.5 + + + + + diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index 38f2d0d5a3..2f86d14cd6 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -7,6 +7,11 @@ models. Because the system uses joints to connect models, the resulting kinematic topology has to be a tree, i.e., kinematic loops are not currently supported. This affects the choice of the parent link, and therefore, the parent model, which is the model that contains the `DetachableJoint` system. +Once detached, the joint can be re-attached by publishing to a topic. +When reattaching, the child model will be attached to the parent model at its +current pose/configuration. To achieve reattachment at a specific pose, the +child model can be positioned accordingly through a set_pose service call prior +to reattaching the joint. For example, [detachable_joint.sdf](https://github.com/gazebosim/gz-sim/blob/ign-gazebo2/examples/worlds/detachable_joint.sdf) demonstrates a four wheel vehicle that holds three objects that are later @@ -36,6 +41,10 @@ need to collide with a parent model or other detached models that have the same parent, the parent model needs to have `` set to true. However, due to an issue in DART, the default physics engine, it is important that none of the parent or child models be in collision in their initial (attached) state. +Furthermore, it is important to note that reattaching a child model is not +currently supported while the child model and parent model are in contact. +Therefore, it is imperative to ensure that there is no collision between the +child and parent model when attempting to perform the reattachment process. The system has the following parameters: @@ -48,6 +57,22 @@ joint. * ``: Name of the link in the `` that will be used as the child link in the detachable joint. -* topic (optional): Topic name to be used for detaching connections. If empty, -a default topic will be created with a pattern -`/model//detachable_joint/detach`. +* `topic` (optional): Topic name to be used for detaching connections. Using + is preferred. If empty, a default topic will be created with a +pattern `/model//detachable_joint/detach`. + +* `detach_topic` (optional): Topic name to be used for detaching connections. + If empty, a default topic will be created with a pattern +`/model//detachable_joint/detach`. 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 re-attaching connections. + If empty, a default topic will be created with a pattern +`/model//detachable_joint/attach`. 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 empty, a default topic will be created with a pattern +`/model//detachable_joint/state`. If multiple detachable plugin is +used in one model, `output_topic` is REQUIRED to publish child models state +individually. diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 61b4844606..0b2b6374fb 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -132,7 +132,7 @@ indicating where the sensor is on the ground. body box1 box_body - /box1/detach + /box1/detach @@ -227,7 +227,7 @@ static model `trigger` by adding the following to `trigger` body box2 box_body - /box2/detach + /box2/detach ```