Highlight selected links in publishRobotState() + improved collision visualization #45
Conversation
| const std::vector<const moveit::core::LinkModel*>& link_names = | ||
| robot_state.getRobotModel()->getLinkModelsWithCollisionGeometry(); | ||
| const std::vector<std::string>& link_names = | ||
| highlight_links.empty() ? robot_state.getRobotModel()->getLinkModelNamesWithCollisionGeometry() : |
There was a problem hiding this comment.
getLinkModelNamesWithCollisionGeometry() may limit you... there is visual geometry that you might want to highlight that isn't listed in collision geometry. can we make this more generic?
There was a problem hiding this comment.
ah i realize this was my previous behavior. now im wondering if it makes sense, but its a bit off topic i guess
There was a problem hiding this comment.
I think using this function is perfectly fine. Links where there is only a visual defined use the underlying geometry as collision shape. I tested this and the function really highlights all links even though there are only visual shapes defined in the urdf.
Improve documentation Co-Authored-By: henningkayser <henningkayser@picknik.ai>
henningkayser
changed the title
|
@davetcoleman I extended this PR with improved collision visualization functions since the code is closely related. |
|
Travis is failing on master, unrelated to this PR. I've attempted to fix here: #47 Merging this now so we can continue to contribute to open source on a client project. |
…visualization (#45) * Highlight only selected links in publishRobotState() * Apply suggestions from code review Improve documentation Co-Authored-By: henningkayser <henningkayser@picknik.ai> * Check for and visualize collisions with checkAndPublishCollision()
|
Cherry picked to master. |
The current implementation of
publishRobotState()allows coloring all links with a specific color.This PR adds an optional argument
highlight_linksfor specifying only a set of link names that should be highlighted with the given color. One example scenario would be to highligh links if they are in collision.I'm also thinking of adding another function
visualizeCollisionResult()or evencheckCollision()that runs a collision check and then highlights the colliding links and publishes contact points usingpublishContactPoints().UPDATE - I extended this PR
checkAndPublishCollision()for performing collision checks and highlighting the resultsCollisionResult::ContactMapwhich allows visualizing contact points for already performed collision checks