From 0d836d5d666c6dcc4eb84562cf839ef32ed7d9af Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sat, 3 Sep 2022 06:50:28 +0000 Subject: [PATCH 01/23] added re-attching feature on the detachable_joint plugin Signed-off-by: Liam Han --- examples/worlds/detachable_joint.sdf | 18 +- .../detachable_joint/DetachableJoint.cc | 63 +++- .../detachable_joint/DetachableJoint.hh | 13 +- test/integration/detachable_joint.cc | 134 +++++++ test/worlds/detachable_joint.sdf | 334 +++++++++++++++++- tutorials/detachable_joints.md | 9 +- 6 files changed, 542 insertions(+), 29 deletions(-) diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index c88799cda7..74c0357b93 100644 --- a/examples/worlds/detachable_joint.sdf +++ b/examples/worlds/detachable_joint.sdf @@ -353,23 +353,29 @@ 1.25 0.3 - + chassis B1 body - /B1/detach + /B1/detach + /B1/attach - + chassis B2 body - /B2/detach + /B2/detach + /B2/attach - + chassis B3 body - /B3/detach + /B3/detach + /B3/attach diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 0946aa9505..0757396972 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -96,15 +96,46 @@ 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); + this->detachTopic = validTopic(detachTopics); + // Setup subscriber for detach topic + this->node.Subscribe( + this->detachTopic, &DetachableJoint::OnDetachRequest, this); + + ignmsg << "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) + + "/attachable_joint/attach"); + this->attachTopic = validTopic(attachTopics); + + // Setup subscriber for attach topic + auto msgCb = std::function( + [this](const auto &) + { + this->attachRequested = true; + }); + + if (!this->node.Subscribe(this->attachTopic, msgCb)) + { + ignerr << "Subscriber could not be created for [attach] topic.\n"; + return; + } + + // Supress Child Warning this->suppressChildWarning = _sdf->Get("suppress_child_warning", this->suppressChildWarning) .first; @@ -118,8 +149,13 @@ void DetachableJoint::PreUpdate( ignition::gazebo::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 +184,9 @@ 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; + igndbg<<"Attaching entity: " << this->detachableJointEntity <initialized) + // only allow detaching if child entity is attached + if (this->isAttached) { if (this->detachRequested && (kNullEntity != this->detachableJointEntity)) { @@ -179,6 +211,7 @@ void DetachableJoint::PreUpdate( _ecm.RequestRemoveEntity(this->detachableJointEntity); this->detachableJointEntity = kNullEntity; this->detachRequested = false; + this->isAttached = false; } } } diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index f52dc9d24a..a5ae248918 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -86,7 +86,10 @@ 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 Whether to suppress warning about missing child model. private: bool suppressChildWarning{false}; @@ -103,14 +106,18 @@ namespace systems /// \brief Whether detachment has been requested private: std::atomic detachRequested{false}; + /// \brief Whether attachment has been requested + private: std::atomic attachRequested{true}; + + /// \brief Whether child entity is attached + private: std::atomic isAttached{false}; + /// \brief Ignition communication node. public: transport::Node node; /// \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 02005b0ae0..6bbfe01651 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -213,3 +213,137 @@ 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("/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; + + // 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, b1Poses.size()); + ASSERT_EQ(nItersAfterDetach, 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_TRUE(abs(vehiclePoses.back().Pos().X() - + 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()); + + // since the two models are attached, the distances traveled by both objects + // should be close. + EXPECT_TRUE(abs(distTraveledB1 - distTraveledVehicle) < 0.001); + } diff --git a/test/worlds/detachable_joint.sdf b/test/worlds/detachable_joint.sdf index ac3be7c50a..eb9f354d61 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 70a2456b8d..c429c619ca 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -7,6 +7,7 @@ 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. For example, [detachable_joint.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/detachable_joint.sdf) demonstrates a four wheel vehicle that holds three objects that are later @@ -48,6 +49,10 @@ 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 +* `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`. + +* `attach_topic` (optional): Topic name to be used for re-attaching connections. + If empty, a default topic will be created with a pattern +`/model//attachable_joint/attach`. From 9471669ee81c0bf465a700e140fa3e299bc986b6 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sat, 3 Sep 2022 07:23:17 +0000 Subject: [PATCH 02/23] fixing white space issue provided by codecheck Signed-off-by: Liam Han --- src/systems/detachable_joint/DetachableJoint.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 0757396972..8db43f294f 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -186,7 +186,8 @@ void DetachableJoint::PreUpdate( this->childLinkEntity, "fixed"})); this->attachRequested = false; this->isAttached = true; - igndbg<<"Attaching entity: " << this->detachableJointEntity <detachableJointEntity + << std::endl; } else { From 2a566bfc103b6611ae6a1d7b41477d56d3a0da94 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sat, 3 Sep 2022 21:05:19 +0000 Subject: [PATCH 03/23] relaxing the tolerance Signed-off-by: Liam Han --- test/integration/detachable_joint.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 6bbfe01651..fa7dbd49f4 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -345,5 +345,5 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) // since the two models are attached, the distances traveled by both objects // should be close. - EXPECT_TRUE(abs(distTraveledB1 - distTraveledVehicle) < 0.001); + EXPECT_TRUE(abs(distTraveledB1 - distTraveledVehicle) < 0.01); } From 1f7c23839647cd5567461d299e6a65cb8ad047ca Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sat, 3 Sep 2022 22:38:13 +0000 Subject: [PATCH 04/23] adding igndbg msg for easier debugging for potential reattach flaky test Signed-off-by: Liam Han --- test/integration/detachable_joint.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index fa7dbd49f4..8d0c6deb1c 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -342,6 +342,9 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) // 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. From 5245a84e58b8b6350346b1ffd3855ca4f8d64c6e Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 6 Sep 2022 23:20:25 +0000 Subject: [PATCH 05/23] adding output publisher to be able to monitor the state of the detachment Signed-off-by: Liam Han --- examples/worlds/detachable_joint.sdf | 15 +++++++ .../detachable_joint/DetachableJoint.cc | 42 ++++++++++++++++++- .../detachable_joint/DetachableJoint.hh | 28 ++++++++++--- tutorials/detachable_joints.md | 4 ++ 4 files changed, 83 insertions(+), 6 deletions(-) diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index 74c0357b93..0bb91b5ac1 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/detached_state + ign topic -e -t /B2/detached_state + ign topic -e -t /B3/detached_state --> @@ -360,6 +372,7 @@ body /B1/detach /B1/attach + /B1/detached_state @@ -368,6 +381,7 @@ body /B2/detach /B2/attach + /B2/detached_state @@ -376,6 +390,7 @@ body /B3/detach /B3/attach + /B3/detached_state diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 8db43f294f..e56bc58e63 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -104,12 +104,13 @@ void DetachableJoint::Configure(const Entity &_entity, detachTopics.push_back("/model/" + this->model.Name(_ecm) + "/detachable_joint/detach"); this->detachTopic = validTopic(detachTopics); + igndbg << "Detach topic is: " << this->detachTopic << std::endl; // Setup subscriber for detach topic this->node.Subscribe( this->detachTopic, &DetachableJoint::OnDetachRequest, this); - ignmsg << "DetachableJoint subscribing to messages on " + igndbg << "DetachableJoint subscribing to messages on " << "[" << this->detachTopic << "]" << std::endl; // Setup attach topic @@ -121,6 +122,7 @@ void DetachableJoint::Configure(const Entity &_entity, attachTopics.push_back("/model/" + this->model.Name(_ecm) + "/attachable_joint/attach"); this->attachTopic = validTopic(attachTopics); + igndbg << "Attach topic is: " << this->attachTopic << std::endl; // Setup subscriber for attach topic auto msgCb = std::function( @@ -135,6 +137,27 @@ void DetachableJoint::Configure(const Entity &_entity, return; } + // Setup output topic + std::vector outputTopics; + if (_sdf->HasElement("output_topic")) + { + outputTopics.push_back(_sdf->Get("output_topic")); + } + outputTopics.push_back("/ouput" + this->model.Name(_ecm) + + "/detached_state"); + this->outputTopic = validTopic(outputTopics); + igndbg << "Output topic is: " << this->outputTopic << std::endl; + + // Setup publisher for output topic + this->outputPub = this->node.Advertise( + this->outputTopic); + if (!this->outputPub) + { + std::cerr << "Error advertising topic [" << this->outputTopic << "]" + << std::endl; + return; + } + // Supress Child Warning this->suppressChildWarning = _sdf->Get("suppress_child_warning", this->suppressChildWarning) @@ -186,6 +209,7 @@ void DetachableJoint::PreUpdate( this->childLinkEntity, "fixed"})); this->attachRequested = false; this->isAttached = true; + this->PublishOutput(this->isAttached); igndbg << "Attaching entity: " << this->detachableJointEntity << std::endl; } @@ -213,10 +237,26 @@ void DetachableJoint::PreUpdate( this->detachableJointEntity = kNullEntity; this->detachRequested = false; this->isAttached = false; + this->PublishOutput(this->isAttached); } } } +////////////////////////////////////////////////// +void DetachableJoint::PublishOutput(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 &) { diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index a5ae248918..4decf78f55 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,14 @@ 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 + /// + /// - `` (optional): Topic name to be used for attaching + /// connections + /// + /// - `` (optional): Topic name to be used for publishing + /// the state of the detachment. /// /// - `` (optional): If true, the system /// will not print a warning message if a child model does not exist yet. @@ -73,6 +82,15 @@ namespace systems const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) final; + /// \brief Ignition communication node. + private: transport::Node node; + + /// \brief A publisher to send state of the detachment + private: ignition::transport::Node::Publisher outputPub; + + /// \brief Helper function to publish the state of the detachment + private: void PublishOutput(bool attached); + /// \brief Callback for detach request topic private: void OnDetachRequest(const msgs::Empty &_msg); @@ -91,6 +109,9 @@ namespace systems /// \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}; @@ -112,9 +133,6 @@ namespace systems /// \brief Whether child entity is attached private: std::atomic isAttached{false}; - /// \brief Ignition communication node. - public: transport::Node node; - /// \brief Whether all parameters are valid and the system can proceed private: bool validConfig{false}; diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index c429c619ca..7decfc44cf 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -56,3 +56,7 @@ as the child link in the detachable joint. * `attach_topic` (optional): Topic name to be used for re-attaching connections. If empty, a default topic will be created with a pattern `/model//attachable_joint/attach`. + +* `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//detached_state`. From 4320488dd065a89ec2e9f38515cfdb37d35d671c Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 6 Sep 2022 23:20:25 +0000 Subject: [PATCH 06/23] adding output publisher to be able to monitor the state of the detachment. Signed-off-by: Liam Han --- examples/worlds/detachable_joint.sdf | 15 +++++++ .../detachable_joint/DetachableJoint.cc | 42 ++++++++++++++++++- .../detachable_joint/DetachableJoint.hh | 28 ++++++++++--- test/integration/detachable_joint.cc | 2 +- tutorials/detachable_joints.md | 4 ++ 5 files changed, 84 insertions(+), 7 deletions(-) diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index 74c0357b93..0bb91b5ac1 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/detached_state + ign topic -e -t /B2/detached_state + ign topic -e -t /B3/detached_state --> @@ -360,6 +372,7 @@ body /B1/detach /B1/attach + /B1/detached_state @@ -368,6 +381,7 @@ body /B2/detach /B2/attach + /B2/detached_state @@ -376,6 +390,7 @@ body /B3/detach /B3/attach + /B3/detached_state diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 8db43f294f..e56bc58e63 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -104,12 +104,13 @@ void DetachableJoint::Configure(const Entity &_entity, detachTopics.push_back("/model/" + this->model.Name(_ecm) + "/detachable_joint/detach"); this->detachTopic = validTopic(detachTopics); + igndbg << "Detach topic is: " << this->detachTopic << std::endl; // Setup subscriber for detach topic this->node.Subscribe( this->detachTopic, &DetachableJoint::OnDetachRequest, this); - ignmsg << "DetachableJoint subscribing to messages on " + igndbg << "DetachableJoint subscribing to messages on " << "[" << this->detachTopic << "]" << std::endl; // Setup attach topic @@ -121,6 +122,7 @@ void DetachableJoint::Configure(const Entity &_entity, attachTopics.push_back("/model/" + this->model.Name(_ecm) + "/attachable_joint/attach"); this->attachTopic = validTopic(attachTopics); + igndbg << "Attach topic is: " << this->attachTopic << std::endl; // Setup subscriber for attach topic auto msgCb = std::function( @@ -135,6 +137,27 @@ void DetachableJoint::Configure(const Entity &_entity, return; } + // Setup output topic + std::vector outputTopics; + if (_sdf->HasElement("output_topic")) + { + outputTopics.push_back(_sdf->Get("output_topic")); + } + outputTopics.push_back("/ouput" + this->model.Name(_ecm) + + "/detached_state"); + this->outputTopic = validTopic(outputTopics); + igndbg << "Output topic is: " << this->outputTopic << std::endl; + + // Setup publisher for output topic + this->outputPub = this->node.Advertise( + this->outputTopic); + if (!this->outputPub) + { + std::cerr << "Error advertising topic [" << this->outputTopic << "]" + << std::endl; + return; + } + // Supress Child Warning this->suppressChildWarning = _sdf->Get("suppress_child_warning", this->suppressChildWarning) @@ -186,6 +209,7 @@ void DetachableJoint::PreUpdate( this->childLinkEntity, "fixed"})); this->attachRequested = false; this->isAttached = true; + this->PublishOutput(this->isAttached); igndbg << "Attaching entity: " << this->detachableJointEntity << std::endl; } @@ -213,10 +237,26 @@ void DetachableJoint::PreUpdate( this->detachableJointEntity = kNullEntity; this->detachRequested = false; this->isAttached = false; + this->PublishOutput(this->isAttached); } } } +////////////////////////////////////////////////// +void DetachableJoint::PublishOutput(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 &) { diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index a5ae248918..4decf78f55 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,14 @@ 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 + /// + /// - `` (optional): Topic name to be used for attaching + /// connections + /// + /// - `` (optional): Topic name to be used for publishing + /// the state of the detachment. /// /// - `` (optional): If true, the system /// will not print a warning message if a child model does not exist yet. @@ -73,6 +82,15 @@ namespace systems const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) final; + /// \brief Ignition communication node. + private: transport::Node node; + + /// \brief A publisher to send state of the detachment + private: ignition::transport::Node::Publisher outputPub; + + /// \brief Helper function to publish the state of the detachment + private: void PublishOutput(bool attached); + /// \brief Callback for detach request topic private: void OnDetachRequest(const msgs::Empty &_msg); @@ -91,6 +109,9 @@ namespace systems /// \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}; @@ -112,9 +133,6 @@ namespace systems /// \brief Whether child entity is attached private: std::atomic isAttached{false}; - /// \brief Ignition communication node. - public: transport::Node node; - /// \brief Whether all parameters are valid and the system can proceed private: bool validConfig{false}; diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 8d0c6deb1c..d999cd323a 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -230,7 +230,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint.sdf"); + 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 diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index c429c619ca..7decfc44cf 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -56,3 +56,7 @@ as the child link in the detachable joint. * `attach_topic` (optional): Topic name to be used for re-attaching connections. If empty, a default topic will be created with a pattern `/model//attachable_joint/attach`. + +* `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//detached_state`. From 02e11c5e50af39b8ac4954763d98e603597b400d Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 12 Sep 2022 18:54:11 +0000 Subject: [PATCH 07/23] fixing codecheck Signed-off-by: Liam Han --- test/integration/detachable_joint.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index d999cd323a..03c659c1ba 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -230,7 +230,8 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer(common::joinPaths("/test", "worlds", "detachable_joint.sdf")); + 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 From aac08c9f86af788e8002b36bdab185e82319e461 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 26 Sep 2022 19:23:07 +0000 Subject: [PATCH 08/23] fixing multiple output topics to have unique child model name Signed-off-by: Liam Han --- src/systems/detachable_joint/DetachableJoint.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index e56bc58e63..3e15128231 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -143,7 +143,7 @@ void DetachableJoint::Configure(const Entity &_entity, { outputTopics.push_back(_sdf->Get("output_topic")); } - outputTopics.push_back("/ouput" + this->model.Name(_ecm) + + outputTopics.push_back(this->childModelName + "/detached_state"); this->outputTopic = validTopic(outputTopics); igndbg << "Output topic is: " << this->outputTopic << std::endl; From b6ccd69470040c9e4973d0e6ae71d4f94ad63a91 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 26 Sep 2022 19:41:45 +0000 Subject: [PATCH 09/23] fixing multiple detach topics to have unique child model name Signed-off-by: Liam Han --- src/systems/detachable_joint/DetachableJoint.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 3e15128231..dad03b0ac2 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -101,7 +101,7 @@ void DetachableJoint::Configure(const Entity &_entity, { detachTopics.push_back(_sdf->Get("detach_topic")); } - detachTopics.push_back("/model/" + this->model.Name(_ecm) + + detachTopics.push_back("/model/" + this->childModelName + "/detachable_joint/detach"); this->detachTopic = validTopic(detachTopics); igndbg << "Detach topic is: " << this->detachTopic << std::endl; From 580b71936011cd135c53466cea093378c918b008 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 26 Sep 2022 20:22:37 +0000 Subject: [PATCH 10/23] reverted back the change. changed the README to reflect that multiple detachable joint plugin in a single model requires detach_topic, attach_topic, and output_topic to control invididual child model Signed-off-by: Liam Han --- examples/worlds/triggered_publisher.sdf | 4 ++-- src/systems/detachable_joint/DetachableJoint.cc | 2 +- tutorials/detachable_joints.md | 10 +++++++--- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 98b7c52c64..fa61f7a7b8 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -393,7 +393,7 @@ 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 dad03b0ac2..3e15128231 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -101,7 +101,7 @@ void DetachableJoint::Configure(const Entity &_entity, { detachTopics.push_back(_sdf->Get("detach_topic")); } - detachTopics.push_back("/model/" + this->childModelName + + detachTopics.push_back("/model/" + this->model.Name(_ecm) + "/detachable_joint/detach"); this->detachTopic = validTopic(detachTopics); igndbg << "Detach topic is: " << this->detachTopic << std::endl; diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index 7decfc44cf..c512465d74 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -51,12 +51,16 @@ as the child link in the detachable joint. * `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`. +`/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//attachable_joint/attach`. +`/model//attachable_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//detached_state`. +`/model//detached_state`. If multiple detachable plugin is +used in one model, `output_topic` is REQUIRED to publish child models state +individually. From e11b10042eb52a373c2bee694ab4ae86492573e6 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 28 Sep 2022 04:23:24 +0000 Subject: [PATCH 11/23] added a gate to not invoke attach/detach if it's alreadyed attached/detached respectively Signed-off-by: Liam Han --- src/systems/detachable_joint/DetachableJoint.cc | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 3e15128231..745f2b3339 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -128,6 +128,10 @@ void DetachableJoint::Configure(const Entity &_entity, auto msgCb = std::function( [this](const auto &) { + if (this->isAttached){ + igndbg << "Already attached" << std::endl; + return; + } this->attachRequested = true; }); @@ -260,6 +264,10 @@ void DetachableJoint::PublishOutput(bool attached) ////////////////////////////////////////////////// void DetachableJoint::OnDetachRequest(const msgs::Empty &) { + if (!this->isAttached){ + igndbg << "Already detached" << std::endl; + return; + } this->detachRequested = true; } From 189847b67f116e8408036d8f5a99bed9e2e26bd1 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 28 Sep 2022 09:00:00 +0000 Subject: [PATCH 12/23] fixed test error Signed-off-by: Liam Han --- test/integration/detachable_joint.cc | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 03c659c1ba..5a361c39ef 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -267,6 +267,10 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) 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"); @@ -275,8 +279,8 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) const std::size_t nItersAfterDetach{100}; this->server->Run(true, nItersAfterDetach, false); - ASSERT_EQ(nItersAfterDetach, b1Poses.size()); - ASSERT_EQ(nItersAfterDetach, vehiclePoses.size()); + 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; From fb4b5b6478652940123599eb42004104aae6baa4 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 4 Oct 2022 22:53:03 +0000 Subject: [PATCH 13/23] changed the output topic variable name and added more comments on the README Signed-off-by: Liam Han --- src/systems/detachable_joint/DetachableJoint.cc | 6 +++--- src/systems/detachable_joint/DetachableJoint.hh | 12 ++++++++---- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 745f2b3339..aad7e89d23 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -213,7 +213,7 @@ void DetachableJoint::PreUpdate( this->childLinkEntity, "fixed"})); this->attachRequested = false; this->isAttached = true; - this->PublishOutput(this->isAttached); + this->PublishJointState(this->isAttached); igndbg << "Attaching entity: " << this->detachableJointEntity << std::endl; } @@ -241,13 +241,13 @@ void DetachableJoint::PreUpdate( this->detachableJointEntity = kNullEntity; this->detachRequested = false; this->isAttached = false; - this->PublishOutput(this->isAttached); + this->PublishJointState(this->isAttached); } } } ////////////////////////////////////////////////// -void DetachableJoint::PublishOutput(bool attached) +void DetachableJoint::PublishJointState(bool attached) { ignition::msgs::StringMsg detachedStateMsg; if (attached) diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index 4decf78f55..cdcd1e0506 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -51,13 +51,17 @@ namespace systems /// creating a fixed joint with a link in the parent model. /// /// - `` (optional): Topic name to be used for detaching - /// connections + /// 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 + /// 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. + /// 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. @@ -89,7 +93,7 @@ namespace systems private: ignition::transport::Node::Publisher outputPub; /// \brief Helper function to publish the state of the detachment - private: void PublishOutput(bool attached); + private: void PublishJointState(bool attached); /// \brief Callback for detach request topic private: void OnDetachRequest(const msgs::Empty &_msg); From bf3c716fcebff3ccf3509b4332ab7e244945171f Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 4 Oct 2022 23:00:34 +0000 Subject: [PATCH 14/23] taking triggered_publisher.sdf file from this current PR Signed-off-by: Liam Han --- examples/worlds/triggered_publisher.sdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index fa61f7a7b8..98b7c52c64 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -393,7 +393,7 @@ start falling. body box1 box_body - /box1/detach + /box1/detach body box2 box_body - /box2/detach + /box2/detach From 80ada95dbfd5fc63208ed2f52dbbd36eb764f21a Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 10 Oct 2022 17:31:24 +0000 Subject: [PATCH 15/23] modifying examples to use detach_topic tag intead and changing ignition->gazebo Signed-off-by: Liam Han --- examples/worlds/triggered_publisher.sdf | 4 ++-- src/systems/detachable_joint/DetachableJoint.hh | 2 +- tutorials/triggered_publisher.md | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 98b7c52c64..fa61f7a7b8 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -393,7 +393,7 @@ 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.hh b/src/systems/detachable_joint/DetachableJoint.hh index cdcd1e0506..ab643f4fcb 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -86,7 +86,7 @@ namespace systems const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) final; - /// \brief Ignition communication node. + /// \brief Gazebo communication node. private: transport::Node node; /// \brief A publisher to send state of the detachment diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index ff4759c3ef..0efc992cc8 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 ``` From 66a2abe5044048b74de8cd4363ddf97bdc57b7a1 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Thu, 17 Nov 2022 01:10:50 +0000 Subject: [PATCH 16/23] add: handle backwards compatibility using tag instead of the updated tag. Signed-off-by: Liam Han --- .../detachable_joint/DetachableJoint.cc | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index aad7e89d23..edab52f6e2 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -106,6 +106,36 @@ void DetachableJoint::Configure(const Entity &_entity, this->detachTopic = validTopic(detachTopics); igndbg << "Detach topic is: " << this->detachTopic << std::endl; + // Warn if DEPRECATED tag is used instead of + if (_sdf->HasElement("topic")) + { + ignerr << " tag is DEPRECATED. Please use instead." + << std::endl; + 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 + { + ignwarn << "Ignoring tag and using tag." + << std::endl; + } + } + else + { + detachTopics.pop_back(); + detachTopics.push_back(_sdf->Get("topic")); + this->detachTopic = validTopic(detachTopics); + ignerr << "Implicitly converted to . " + << "Detach topic is: " << this->detachTopic << std::endl; + } + } + // Setup subscriber for detach topic this->node.Subscribe( this->detachTopic, &DetachableJoint::OnDetachRequest, this); From 4154296935bea0590eba0e61795cd0d0372e8e69 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sat, 22 Apr 2023 13:20:07 -0700 Subject: [PATCH 17/23] addressing PR review Signed-off-by: Liam Han --- examples/worlds/detachable_joint.sdf | 6 +++--- .../detachable_joint/DetachableJoint.cc | 18 ++++++++---------- .../detachable_joint/DetachableJoint.hh | 2 +- test/integration/detachable_joint.cc | 4 ++-- tutorials/detachable_joints.md | 4 ++-- 5 files changed, 16 insertions(+), 18 deletions(-) diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index 0bb91b5ac1..18675975c4 100644 --- a/examples/worlds/detachable_joint.sdf +++ b/examples/worlds/detachable_joint.sdf @@ -372,7 +372,7 @@ body /B1/detach /B1/attach - /B1/detached_state + /B1/state @@ -381,7 +381,7 @@ body /B2/detach /B2/attach - /B2/detached_state + /B2/state @@ -390,7 +390,7 @@ body /B3/detach /B3/attach - /B3/detached_state + /B3/state diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index edab52f6e2..1e81735bc9 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -106,11 +106,8 @@ void DetachableJoint::Configure(const Entity &_entity, this->detachTopic = validTopic(detachTopics); igndbg << "Detach topic is: " << this->detachTopic << std::endl; - // Warn if DEPRECATED tag is used instead of if (_sdf->HasElement("topic")) { - ignerr << " tag is DEPRECATED. Please use instead." - << std::endl; if (_sdf->HasElement("detach_topic")) { if (_sdf->Get("topic") != @@ -128,10 +125,9 @@ void DetachableJoint::Configure(const Entity &_entity, } else { - detachTopics.pop_back(); - detachTopics.push_back(_sdf->Get("topic")); + detachTopics.insert(detachTopics.begin(), _sdf->Get("topic")); this->detachTopic = validTopic(detachTopics); - ignerr << "Implicitly converted to . " + ignwarn << "Implicitly converted to . " << "Detach topic is: " << this->detachTopic << std::endl; } } @@ -150,7 +146,7 @@ void DetachableJoint::Configure(const Entity &_entity, attachTopics.push_back(_sdf->Get("attach_topic")); } attachTopics.push_back("/model/" + this->model.Name(_ecm) + - "/attachable_joint/attach"); + "/detachable_joint/attach"); this->attachTopic = validTopic(attachTopics); igndbg << "Attach topic is: " << this->attachTopic << std::endl; @@ -177,8 +173,10 @@ void DetachableJoint::Configure(const Entity &_entity, { outputTopics.push_back(_sdf->Get("output_topic")); } - outputTopics.push_back(this->childModelName + - "/detached_state"); + + outputTopics.push_back("/model/" + this->childModelName + + "/detachable_joint/state"); + this->outputTopic = validTopic(outputTopics); igndbg << "Output topic is: " << this->outputTopic << std::endl; @@ -187,7 +185,7 @@ void DetachableJoint::Configure(const Entity &_entity, this->outputTopic); if (!this->outputPub) { - std::cerr << "Error advertising topic [" << this->outputTopic << "]" + ignerr << "Error advertising topic [" << this->outputTopic << "]" << std::endl; return; } diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index ab643f4fcb..7de771437d 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -90,7 +90,7 @@ namespace systems private: transport::Node node; /// \brief A publisher to send state of the detachment - private: ignition::transport::Node::Publisher outputPub; + private: transport::Node::Publisher outputPub; /// \brief Helper function to publish the state of the detachment private: void PublishJointState(bool attached); diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 5a361c39ef..2b461916e8 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -285,8 +285,8 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) // 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_TRUE(abs(vehiclePoses.back().Pos().X() - - b1Poses.back().Pos().X()) - defaultDist < 0.0001); + EXPECT_NEAR(vehiclePoses.back().Pos().X(), + abs(b1Poses.back().Pos().X()) - defaultDist, 0.0001); // clear the vectors b1Poses.clear(); diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index c512465d74..1ef18ca251 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -56,11 +56,11 @@ used in one model, `detach_topic` is REQUIRED to detach child models individuall * `attach_topic` (optional): Topic name to be used for re-attaching connections. If empty, a default topic will be created with a pattern -`/model//attachable_joint/attach`. If multiple detachable plugin is +`/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//detached_state`. If multiple detachable plugin is +`/model//detachable_joint/state`. If multiple detachable plugin is used in one model, `output_topic` is REQUIRED to publish child models state individually. From ef7fafa1d7dd48917b774fa2458c59975ceca252 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 25 Apr 2023 10:07:29 -0700 Subject: [PATCH 18/23] addressing codecheck error Signed-off-by: Liam Han --- src/systems/detachable_joint/DetachableJoint.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 1e81735bc9..9cf246248b 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -125,7 +125,8 @@ void DetachableJoint::Configure(const Entity &_entity, } else { - detachTopics.insert(detachTopics.begin(), _sdf->Get("topic")); + detachTopics.insert(detachTopics.begin(), + _sdf->Get("topic")); this->detachTopic = validTopic(detachTopics); ignwarn << "Implicitly converted to . " << "Detach topic is: " << this->detachTopic << std::endl; From 79776c6ebfb071ce2e5fab4735c3739da84e2440 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sun, 21 May 2023 22:01:01 -0700 Subject: [PATCH 19/23] addressing PR review Signed-off-by: Liam Han --- .../detachable_joint/DetachableJoint.cc | 23 +++++++++++++++---- .../detachable_joint/DetachableJoint.hh | 3 +++ tutorials/detachable_joints.md | 4 ++++ 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 9cf246248b..60268132ae 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -103,8 +103,6 @@ void DetachableJoint::Configure(const Entity &_entity, } detachTopics.push_back("/model/" + this->model.Name(_ecm) + "/detachable_joint/detach"); - this->detachTopic = validTopic(detachTopics); - igndbg << "Detach topic is: " << this->detachTopic << std::endl; if (_sdf->HasElement("topic")) { @@ -127,12 +125,17 @@ void DetachableJoint::Configure(const Entity &_entity, { detachTopics.insert(detachTopics.begin(), _sdf->Get("topic")); - this->detachTopic = validTopic(detachTopics); - ignwarn << "Implicitly converted to . " - << "Detach topic is: " << this->detachTopic << std::endl; } } + 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); @@ -149,6 +152,11 @@ void DetachableJoint::Configure(const Entity &_entity, 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 @@ -179,6 +187,11 @@ void DetachableJoint::Configure(const Entity &_entity, "/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 diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index 7de771437d..ce0c630c4d 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -50,6 +50,9 @@ 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. + /// 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. diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index 1ef18ca251..9c42621615 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -49,6 +49,10 @@ 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. 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 From a002acb292ac395ee3b8e260a31b5955b9556701 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Sun, 21 May 2023 22:09:35 -0700 Subject: [PATCH 20/23] modified the ignwarn -> igndbg since using topic tag shouldn't throw any warnings in this version Signed-off-by: Liam Han --- src/systems/detachable_joint/DetachableJoint.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 60268132ae..c75d4a1285 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -117,7 +117,7 @@ void DetachableJoint::Configure(const Entity &_entity, } else { - ignwarn << "Ignoring tag and using tag." + igndbg << "Ignoring tag and using tag." << std::endl; } } From da051bc90a14a45d9c4ea7c77339f0d6dddd4af0 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 22 May 2023 14:28:09 -0700 Subject: [PATCH 21/23] removed the '/'for compatibility issue Signed-off-by: Liam Han --- test/integration/detachable_joint.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 2b461916e8..a2c42cf59b 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -63,7 +63,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(StartConnected)) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint.sdf"); + this->StartServer("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 @@ -141,7 +141,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer("/test/worlds/detachable_joint.sdf"); + this->StartServer("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 @@ -230,7 +230,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer(common::joinPaths("/test", "worlds", + this->StartServer(common::joinPaths("test", "worlds", "detachable_joint.sdf")); // A lambda that takes a model name and a mutable reference to a vector of From 31e3fe714bc054760b6dfc2b7acfdb4d3e535a2b Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 22 May 2023 14:44:12 -0700 Subject: [PATCH 22/23] reverting back the '/' Signed-off-by: Liam Han --- test/integration/detachable_joint.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index a2c42cf59b..2b461916e8 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -63,7 +63,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(StartConnected)) { using namespace std::chrono_literals; - this->StartServer("test/worlds/detachable_joint.sdf"); + this->StartServer("/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 @@ -141,7 +141,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer("test/worlds/detachable_joint.sdf"); + this->StartServer("/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 @@ -230,7 +230,7 @@ TEST_F(DetachableJointTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) { using namespace std::chrono_literals; - this->StartServer(common::joinPaths("test", "worlds", + this->StartServer(common::joinPaths("/test", "worlds", "detachable_joint.sdf")); // A lambda that takes a model name and a mutable reference to a vector of From 2ff8f529fa0eed663be6879e8779bedb0e2c7475 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Tue, 23 May 2023 22:41:25 -0700 Subject: [PATCH 23/23] adding more comments for the tutorials for clarification Signed-off-by: Liam Han --- examples/worlds/detachable_joint.sdf | 6 +++--- tutorials/detachable_joints.md | 8 ++++++++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index 18675975c4..f05358687e 100644 --- a/examples/worlds/detachable_joint.sdf +++ b/examples/worlds/detachable_joint.sdf @@ -22,9 +22,9 @@ To monitor the state of each breadcrumbs - ign topic -e -t /B1/detached_state - ign topic -e -t /B2/detached_state - ign topic -e -t /B3/detached_state + ign topic -e -t /B1/state + ign topic -e -t /B2/state + ign topic -e -t /B3/state --> diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index 9c42621615..4c162763db 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -8,6 +8,10 @@ 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/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/detachable_joint.sdf) demonstrates a four wheel vehicle that holds three objects that are later @@ -37,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: