-
Notifications
You must be signed in to change notification settings - Fork 340
Suggestion for new dynamic detachable joint plugin (Issue #2362) #3086
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: gz-sim10
Are you sure you want to change the base?
Suggestion for new dynamic detachable joint plugin (Issue #2362) #3086
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It will be great to add some details in the header file as documentation
using namespace systems; | ||
|
||
///////////////////////////////////////////////// | ||
void DynamicDetachableJoint::Configure(const Entity &_entity, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
include ""
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
hi! could you please clarify what header is missing here?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I have added these comments in the header file similar to what was done in the existing DetachableJoint.hh plugin. If you could clarify if i need to add more detailed comments with usage example in the .cc source file above the Configure function then I shall do that.
EntityComponentManager &_ecm, | ||
EventManager &/*_eventMgr*/) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
include header for EntityComponentManager
and EventManager
|
||
if (_sdf->HasElement("parent_link")) | ||
{ | ||
auto parentLinkName = _sdf->Get<std::string>("parent_link"); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
include <string>
#include <gz/plugin/Register.hh> | ||
#include <gz/transport/Node.hh> | ||
|
||
#include <gz/common/Profiler.hh> | ||
|
||
#include <sdf/Element.hh> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
alphabetize
gzdbg << "using service: " | ||
<< this->serviceName << std::endl; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
gzdbg << "using service: " | |
<< this->serviceName << std::endl; | |
gzdbg << "Using service: " << this->serviceName << std::endl; |
gzwarn << "Child Model " << this->childModelName | ||
<< " could not be found.\n"; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
gzwarn << "Child Model " << this->childModelName | |
<< " could not be found.\n"; | |
gzwarn << "Child Model " << this->childModelName | |
<< " could not be found.\n"; |
_res.set_message("Attached to child model " + this->childModelName + | ||
" at link " + this->childLinkName + "."); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
_res.set_message("Attached to child model " + this->childModelName + | |
" at link " + this->childLinkName + "."); | |
_res.set_message("Attached to child model " + this->childModelName + | |
" at link " + this->childLinkName + "."); |
EntityComponentManager &_ecm, | ||
EventManager &_eventMgr) final; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
include headers for EntityComponentManager
and EventManager
private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, | ||
gz::msgs::AttachDetachResponse &_res); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, | |
gz::msgs::AttachDetachResponse &_res); | |
private: bool OnServiceRequest(const gz::msgs::AttachDetachRequest &_req, | |
gz::msgs::AttachDetachResponse &_res); |
private: Entity detachableJointEntity{kNullEntity}; | ||
|
||
/// \brief Whether detachment has been requested | ||
private: std::atomic<bool> detachRequested{false}; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
include <atomic>
Thanks for this contribution @AdarshKaran! Please sign off your commits. You can see how here: https://github.com/gazebosim/gz-sim/pull/3086/checks?check_run_id=50152759632 |
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
…, added clearer loggin Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
361af45
to
33c1fd6
Compare
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
I have addressed the comments. Once my doubt regarding the documentation comment is clarified, I'll add that commit. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks for the contribution! This can be super useful.
I'm being a bit picky, but:
(1) We should at least have an integration test for this system.
(2) Check indentation + Address the comments I've left for you.
(3) You may need to use a lock instead of atomics as we are configuring multiple parameters at once.
<< std::endl; | ||
} | ||
else | ||
{ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Knit: I recommend applying the error-first pattern (also called "happy path" programming) here. By checking for and returning early on error conditions, we can significantly reduce code nesting and enhance the overall readability.
<< " could not be found.\n"; | ||
} | ||
} | ||
else if (!this->suppressChildWarning) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Knit: Why do we need this. Can't we just rely on gazebo's verbosity level?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I actually followed the same pattern as the existing Detachable Joint cc for this.
https://github.com/gazebosim/gz-sim/blob/5dfe0ad1e6e01b8ddb915981e6b3da2efd3cf673/src/systems/detachable_joint/DetachableJoint.cc#L306C1-L311C2
Do we still keep it or remove it for now?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah, that plugin does need a refactor.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
So, do you suggest to remove suppressChildWarning
and instead just have
// if child model is not found
if (kNullEntity == modelEntity)
{
gzdbg << "Child Model " << this->childModelName
<< " could not be found.\n";
return;
}
gzdbg instead of gzwarn?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
No keep it as gzwarn
just dont bother with the suppress child option.
if (this->isAttached) | ||
{ | ||
_res.set_success(false); | ||
_res.set_message("Already attached to child model " + this->attachedChildModelName + |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Knit: Indentation issue
gz::msgs::StringMsg detachedStateMsg; | ||
if (attached) | ||
{ | ||
detachedStateMsg.set_data("attached to " + |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why are we publishing a custom message with custom semantics? I'm not sure we should publish this at all.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I followed the same pattern as DetachableJoint.cc
do we still remove this too?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
DetachableJoint.cc
publishes only 2 types of outputs: "attached" or "detached" (if I were redesigning this I'd just publish a bool), in your case you publish "attached to ..." . If I were writing a parser to interact with your software I'd have to look up this magic string. Its much better to either publish an empty string or just the linked name so end users dont have to write custom parsers. Alternatively publish the entity/knullentity that the object is attached to.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
That's a good point. I did not think from that angle. But how exactly would we be able to differentiate bw attached and detached state in the case of an empty string or which model it is attached to in case of link name(when different models have same link names)?
So maybe in that case, using the entity/knullentity like you said is a better option?
void DynamicDetachableJoint::PublishJointState(bool attached)
{
gz::msgs::Entity stateMsg;
if (attached)
{
stateMsg.set_id(this->childLinkEntity);
stateMsg.set_type(gz::msgs::Entity::LINK);
}
else
{
stateMsg.set_id(kNullEntity);
stateMsg.set_type(gz::msgs::Entity::NONE);
}
this->outputPub.Publish(stateMsg);
}
and in the topic echo
id: 29
type: LINK
id: 25
type: LINK
when i do
ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_2', child_link_name: 'body', command: 'attach'}"
ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_2', child_link_name: 'body', command: 'detach'}"
ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_1', child_link_name: 'body', command: 'attach'}"
ros2 service call /payload/attach_detach ros_gz_interfaces/srv/AttachDetach "{child_model_name: 'can_1', child_link_name: 'body', command: 'detach'}"
so basically we could extract the model name from the link entity and in detach state we publish a knullentity so it's empty.
does this change seem good?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
#include <gz/msgs/entity.pb.h>
Just this extra dependency though
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah that seems reasonable to avoid any sense of ambiguity.
} | ||
|
||
////////////////////////////////////////////////// | ||
void DynamicDetachableJoint::OnAttachRequest(const msgs::Empty &) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why are we passing an empty message for no reason? We don't actually need this method.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I actually kept the two function just to like separate concerns and to also keep it extensible in the future. But, if its better to simplify it then I'll remove it for now. Like you said it actually makes more sense to remove since there is an unwanted dependency on the empty proto msg.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I suggest removing it for now
this->childModelName = _req.child_model_name(); | ||
this->childLinkName = _req.child_link_name(); | ||
this->OnAttachRequest(msgs::Empty()); | ||
_res.set_success(true); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
knit: Indentation
// set the child model and link names from the request | ||
this->childModelName = _req.child_model_name(); | ||
this->childLinkName = _req.child_link_name(); | ||
this->OnAttachRequest(msgs::Empty()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
directly assign the this->attachRequested = true;
without calling the method,
} | ||
|
||
// set the child model and link names from the request | ||
this->childModelName = _req.child_model_name(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Knit: I don't think atomics are sufficient here. You do need to use a lock to guarantee data consistency between
this->childModelName
, this->childModelName
and this->attachRequested
.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Regarding mutex locks, I haven't worked much with them before so please guide me here.
@@ -158,6 +159,8 @@ void DynamicDetachableJoint::PreUpdate(
EntityComponentManager &_ecm)
{
GZ_PROFILE("DynamicDetachableJoint::PreUpdate");
+ std::lock_guard<std::mutex> lock(this->mutex);
+
// only allow attaching if child entity is detached
if (this->validConfig && !this->isAttached)
{
@@ -265,6 +268,7 @@ bool DynamicDetachableJoint::OnServiceRequest(const gz::msgs::AttachDetachReques
gz::msgs::AttachDetachResponse &_res)
{
GZ_PROFILE("DynamicDetachableJoint::OnServiceRequest");
+ std::lock_guard<std::mutex> lock(this->mutex);
// Check if the request is valid
if (_req.child_model_name().empty() || _req.child_link_name().empty() )
is it right to add them to the top of PreUpdate loop?
and for the header file, I have removed atomic
/// \brief Whether detachment has been requested
- private: std::atomic<bool> detachRequested{false};
+ private: bool detachRequested{false};
/// \brief Whether attachment has been requested
- private: std::atomic<bool> attachRequested{false};
+ private: bool attachRequested{false};
/// \brief Whether child entity is attached
- private: std::atomic<bool> isAttached{false};
+ private: bool isAttached{false};
/// \brief Whether all parameters are valid and the system can proceed
private: bool validConfig{false};
+ /// \brief Mutex to protect access to member variables
+ private: std::mutex mutex;
};
}
}
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah this approach will work. Although it may be a bit heavy as ideally you'd only lock for the time period which you need access to the variable for. One option is to create a scope within preupdate, lock that scope then copy out the necessary details. For our purposes, I don't think it matters.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
alright then. Thank you! I'll push these changes
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
I have one more ask that falls outside of the integration test. Please add a demo world of some form so I/users can test it. |
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
_res.set_success(false); | ||
_res.set_message("Already attached to child model " + this->attachedChildModelName + | ||
" at link " + this->attachedChildLinkName + "."); | ||
gzdbg << "Already attached" << std::endl; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Might be worth being a bit more descriptive with the error message. Mention the linkName.
} | ||
|
||
// set the child model and link names from the request | ||
this->childModelName = _req.child_model_name(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah this approach will work. Although it may be a bit heavy as ideally you'd only lock for the time period which you need access to the variable for. One option is to create a scope within preupdate, lock that scope then copy out the necessary details. For our purposes, I don't think it matters.
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Yes. I will look into it |
There is actually one issue. When i give attach request and specify a |
Yes, this is one of the less-than-ideal side effects of the way we mix pub/sub with ECM in Gazebo. I think "Attach request accepted" may be a reasonable thing and then log an error. If need be we could publish to a seperate topic, but I'd leave that as "future work". |
Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Done! Please let me know if it looks good. I have also added some additional logs and errors for more clarity |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I have a quick question here. It seems like I forgot to update the review.
In general the code looks ok but I cannot approve it without examples/tests.
{ | ||
GZ_PROFILE("DynamicDetachableJoint::PreUpdate"); | ||
std::lock_guard<std::mutex> lock(this->mutex); | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A quick question : is it intended that the plugin continues to work while paused?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't think it should work when the sim is paused. Should we track if it is paused and return in that case (following the error first approach)?
…tests Signed-off-by: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
🎉 New feature
Closes #2362
Summary
Created a
dynamic_detachable_joint
system as an extension of the detachable joint system where thechildModelName
andchildLinkName
can now be passed through a gz service or a ROS2 service (a bridge has also been added for this). Acommand
:attach
ordetach
can be specified in the service call. I have also added anattachDistance
option that allows you to specify how far the child should be to be considered attached.Most of the code is based on the already existing DetachableJoint Plugin logic
Test it
Adding the plugin to robot model URDF/SDF
GZ Service call command
ROS2 Service call command
Checklist
codecheck
passed (See contributing)Generated-by: Remove this if GenAI was not used.
Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining
Signed-off-by
andGenerated-by
messages.