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 all commits
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
33 changes: 31 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 @@ -485,6 +485,21 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
*/
bool publishWorkspaceParameters(const moveit_msgs::WorkspaceParameters& params);

/**
* \brief Check if the robot state is in collision inside the planning scene and visualize the result.
* If the state is not colliding only the robot state is published. If there is a collision the colliding
* links are ghlighted and the contact points are visualized with markers.
* \param robot_state - The robot state to check for collisions
* \param planning_scene - The planning scene to use for collision checks
* \param highlight_link_color - The color to use for highligting colliding links
* \param contact_point_color - The color to use for contact point markers
* \result - True if there is a collision
*/
bool checkAndPublishCollision(const moveit::core::RobotState& robot_state,
const planning_scene::PlanningScene* planning_scene,
const rviz_visual_tools::colors& highlight_link_color = rviz_visual_tools::RED,
const rviz_visual_tools::colors& contact_point_color = rviz_visual_tools::PURPLE);

/**
* \brief Given a planning scene and robot state, publish any collisions
* \param robot_state
Expand All @@ -496,6 +511,17 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
const planning_scene::PlanningScene* planning_scene,
const rviz_visual_tools::colors& color = rviz_visual_tools::RED);

/**
* \brief Given a contact map and planning scene, publish the contact points
* \param contacts - The populated contacts to visualize
* \param planning_scene
* \param color - display color of markers
* \return true on success
*/
bool publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts,
const planning_scene::PlanningScene* planning_scene,
const rviz_visual_tools::colors& color = rviz_visual_tools::RED);

/**
* \brief Move a joint group in MoveIt for visualization
* make sure you have already set the planning group name
Expand Down Expand Up @@ -607,11 +633,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_links - if the |color| is not |DEFAULT|, allows selective robot links to be highlighted. by default (empty) all links are highlighted
*/
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
64 changes: 55 additions & 9 deletions src/moveit_visual_tools.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1057,6 +1057,33 @@ bool MoveItVisualTools::publishWorkspaceParameters(const moveit_msgs::WorkspaceP
"Planning_Workspace", 1);
}

bool MoveItVisualTools::checkAndPublishCollision(const moveit::core::RobotState& robot_state,
const planning_scene::PlanningScene* planning_scene,
const rviz_visual_tools::colors& highlight_link_color,
const rviz_visual_tools::colors& contact_point_color)
{
// Compute the contacts if any
collision_detection::CollisionRequest c_req;
collision_detection::CollisionResult c_res;
c_req.contacts = true;
c_req.max_contacts = 10;
c_req.max_contacts_per_pair = 3;
c_req.verbose = true;

// Check for collisions
planning_scene->checkCollision(c_req, c_res, robot_state);
std::vector<std::string> highlight_links;
for (const auto& contact : c_res.contacts)
{
highlight_links.push_back(contact.first.first);
highlight_links.push_back(contact.first.second);
}

publishRobotState(robot_state, highlight_link_color, highlight_links);
publishContactPoints(c_res.contacts, planning_scene, contact_point_color);
return c_res.collision;
}

bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& robot_state,
const planning_scene::PlanningScene* planning_scene,
const rviz_visual_tools::colors& color)
Expand All @@ -1071,12 +1098,18 @@ bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& rob

// Check for collisions
planning_scene->checkCollision(c_req, c_res, robot_state);
return publishContactPoints(c_res.contacts, planning_scene, color);
}

bool MoveItVisualTools::publishContactPoints(const collision_detection::CollisionResult::ContactMap& contacts,
const planning_scene::PlanningScene* planning_scene,
const rviz_visual_tools::colors& color)
{
// Display
if (c_res.contact_count > 0)
if (!contacts.empty())
{
visualization_msgs::MarkerArray arr;
collision_detection::getCollisionMarkersFromContacts(arr, planning_scene->getPlanningFrame(), c_res.contacts);
collision_detection::getCollisionMarkersFromContacts(arr, planning_scene->getPlanningFrame(), contacts);
ROS_INFO_STREAM_NAMED(LOGNAME, "Completed listing of explanations for invalid states.");

// Check for markers
Expand All @@ -1087,6 +1120,7 @@ bool MoveItVisualTools::publishContactPoints(const moveit::core::RobotState& rob
for (std::size_t i = 0; i < arr.markers.size(); ++i)
{
arr.markers[i].ns = "Collision";
arr.markers[i].id = i;
arr.markers[i].scale.x = 0.04;
arr.markers[i].scale.y = 0.04;
arr.markers[i].scale.z = 0.04;
Expand Down Expand Up @@ -1395,26 +1429,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 rather than the cached solid robot messages
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 +1465,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 +1495,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