Skip to content
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.

Sign up for GitHub

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

Jump to bottom

Highlight selected links in publishRobotState() + improved collision visualization #45

Merged
merged 3 commits into from
Apr 22, 2019
Merged

Highlight selected links in publishRobotState() + improved collision visualization #45

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1138273
Highlight only selected links in publishRobotState()
henningkayser Mar 27, 2019
190983b
Apply suggestions from code review
davetcoleman Apr 2, 2019
3ef4082
Check for and visualize collisions with checkAndPublishCollision()
henningkayser Mar 29, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 5 additions & 2 deletions include/moveit_visual_tools/moveit_visual_tools.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -607,11 +607,14 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
* To use, add a RobotState marker to Rviz and subscribe to the DISPLAY_ROBOT_STATE_TOPIC, above
* \param robot_state - joint values of robot
* \param color - how to highlight the robot (solid-ly) if desired, default keeps color as specified in URDF
* \param highlight_inks - names of robot link to highlight, by default (empty) all links are highlighted
henningkayser marked this conversation as resolved.
Show resolved Hide resolved
*/
bool publishRobotState(const moveit::core::RobotState& robot_state,
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT);
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
const std::vector<std::string>& highlight_links = {});
bool publishRobotState(const moveit::core::RobotStatePtr& robot_state,
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT);
const rviz_visual_tools::colors& color = rviz_visual_tools::DEFAULT,
const std::vector<std::string>& highlight_links = {});

/**
* \brief Fake removing a Robot State display in Rviz by simply moving it very far away
Expand Down
26 changes: 19 additions & 7 deletions src/moveit_visual_tools.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1395,26 +1395,34 @@ bool MoveItVisualTools::publishRobotState(const std::vector<double> joint_positi
}

bool MoveItVisualTools::publishRobotState(const robot_state::RobotStatePtr& robot_state,
const rviz_visual_tools::colors& color)
const rviz_visual_tools::colors& color,
const std::vector<std::string>& highlight_links)
{
return publishRobotState(*robot_state.get(), color);
return publishRobotState(*robot_state.get(), color, highlight_links);
}

bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_state,
const rviz_visual_tools::colors& color)
const rviz_visual_tools::colors& color,
const std::vector<std::string>& highlight_links)
{
// when only a subset of links should be colored, the default message is used
henningkayser marked this conversation as resolved.
Show resolved Hide resolved
rviz_visual_tools::colors base_color = color;
if (!highlight_links.empty())
base_color = rviz_visual_tools::DEFAULT;

// Reference to the correctly colored version of message (they are cached)
// May not exist yet but this will create it
moveit_msgs::DisplayRobotState& display_robot_msg = display_robot_msgs_[color];
moveit_msgs::DisplayRobotState& display_robot_msg = display_robot_msgs_[base_color];

// Check if a robot state message already exists for this color
if (display_robot_msg.highlight_links.size() == 0) // has not been colored yet, lets create that
{
if (color != rviz_visual_tools::DEFAULT) // ignore color highlights when set to default
{
// Get links names
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() :
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ah i realize this was my previous behavior. now im wondering if it makes sense, but its a bit off topic i guess

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

highlight_links;
display_robot_msg.highlight_links.resize(link_names.size());

// Get color
Expand All @@ -1423,7 +1431,7 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s
// Color every link
for (std::size_t i = 0; i < link_names.size(); ++i)
{
display_robot_msg.highlight_links[i].id = link_names[i]->getName();
display_robot_msg.highlight_links[i].id = link_names[i];
display_robot_msg.highlight_links[i].color = color_rgba;
}
}
Expand Down Expand Up @@ -1453,6 +1461,10 @@ bool MoveItVisualTools::publishRobotState(const robot_state::RobotState& robot_s
pub_robot_state_.publish(display_robot_msg);
ros::spinOnce();

// remove highlight links from default message
if (!highlight_links.empty())
display_robot_msg.highlight_links.clear();

return true;
}

Expand Down