// ******************************************************************************************
// Build the robot links connection graph and then check for links with no geomotry
// ******************************************************************************************
void computeConnectionGraph(const robot_model::LinkModel *start_link, LinkGraph &link_graph)
{
link_graph.clear(); // make sure the edges structure is clear
// Recurively build adj list of link connections
computeConnectionGraphRec(start_link, link_graph);
// Repeatidly check for links with no geometry and remove them, then re-check until no more removals are detected
bool update = true; // track if a no geometry link was found
while (update)
{
update = false; // default
// Check if each edge has a shape
for (LinkGraph::const_iterator edge_it = link_graph.begin() ; edge_it != link_graph.end() ; ++edge_it)
{
if (edge_it->first->getShapes().empty()) // link in adjList "link_graph" does not have shape, remove!
{
// Temporary list for connected links
std::vector<const robot_model::LinkModel*> temp_list;
// Copy link's parent and child links to temp_list
for (std::set<const robot_model::LinkModel*>::const_iterator adj_it = edge_it->second.begin();
adj_it != edge_it->second.end();
++adj_it)
{
temp_list.push_back(*adj_it);
}
// Make all preceeding and succeeding links to the no-shape link fully connected
// so that they don't collision check with themselves
for (std::size_t i = 0 ; i < temp_list.size() ; ++i)
{
for (std::size_t j = i + 1 ; j < temp_list.size() ; ++j)
{
// for each LinkModel in temp_list, find its location in the link_graph structure and insert the rest
// of the links into its unique set.
// if the LinkModel is not already in the set, update is set to true so that we keep searching
if (link_graph[temp_list[i]].insert(temp_list[j]).second)
update = true;
if (link_graph[temp_list[j]].insert(temp_list[i]).second)
update = true;
}
}
}
}
}
//ROS_INFO("Generated connection graph with %d links", int(link_graph.size()));
}
// ******************************************************************************************
// Disable collision checking for adjacent links, or adjacent with no geometry links between them
// ******************************************************************************************
unsigned int disableAdjacentLinks(planning_scene::PlanningScene &scene, LinkGraph &link_graph, LinkPairMap &link_pairs)
{
int num_disabled = 0;
for (LinkGraph::const_iterator link_graph_it = link_graph.begin() ; link_graph_it != link_graph.end() ; ++link_graph_it)
{
// disable all connected links to current link by looping through them
for (std::set<const robot_model::LinkModel*>::const_iterator adj_it = link_graph_it->second.begin();
adj_it != link_graph_it->second.end();
++adj_it)
{
//ROS_INFO("Disabled %s to %s", link_graph_it->first->getName().c_str(), (*adj_it)->getName().c_str() );
// Check if either of the links have no geometry. If so, do not add (are we sure?)
if ( !link_graph_it->first->getShapes().empty() && !(*adj_it)->getShapes().empty() ) // both links have geometry
{
num_disabled += setLinkPair( link_graph_it->first->getName(), (*adj_it)->getName(), ADJACENT, link_pairs );
// disable link checking in the collision matrix
scene.getAllowedCollisionMatrixNonConst().setEntry( link_graph_it->first->getName(), (*adj_it)->getName(), true);
}
}
}
//ROS_INFO("Disabled %d adjancent link pairs from collision checking", num_disabled);
return num_disabled;
}