Skip to content

Commit

Permalink
Update SceneGraph get shortest path to use an undirected graph
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Oct 23, 2021
1 parent c8c5b29 commit 3f1c049
Show file tree
Hide file tree
Showing 4 changed files with 125 additions and 62 deletions.
24 changes: 12 additions & 12 deletions tesseract_scene_graph/include/tesseract_scene_graph/graph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -257,13 +257,13 @@ class SceneGraph

/**
* @brief Set whether a link should be considered during collision checking
* @param enabled True if should be condisdered during collision checking, otherwise false
* @param enabled True if should be considered during collision checking, otherwise false
*/
void setLinkCollisionEnabled(const std::string& name, bool enabled);

/**
* @brief Get whether a link should be considered during collision checking
* @return True if should be condisdered during collision checking, otherwise false
* @return True if should be considered during collision checking, otherwise false
*/
bool getLinkCollisionEnabled(const std::string& name) const;

Expand Down Expand Up @@ -360,7 +360,7 @@ class SceneGraph
* @brief Disable collision between two collision objects
* @param link_name1 Collision object name
* @param link_name2 Collision object name
* @param reason The reason for disabling collison
* @param reason The reason for disabling collision
*/
void addAllowedCollision(const std::string& link_name1, const std::string& link_name2, const std::string& reason);

Expand Down Expand Up @@ -492,7 +492,7 @@ class SceneGraph

/**
* @brief Get all children link names for the given joint names
* @todo Need to create custom vistor so already process joint_names do not get processed again.
* @todo Need to create custom visitor so already process joint_names do not get processed again.
* @param names Name of joints
* @return A vector of child link names
*/
Expand Down Expand Up @@ -530,11 +530,11 @@ class SceneGraph
/**
* @brief Merge a graph into the current graph
* @param scene_graph Const ref to the graph to be merged (said graph will be copied)
* @param prefix string Will be prepended to every link and joint of the merged graph
* @param prefix string Will prepend to every link and joint of the merged graph
* @return Return False if any link or joint name collides with current environment, otherwise True
* Merge a subgraph into the current environment, considering that the root of the merged graph is attached to the
* root of the environment by a fixed joint and no displacement. Every joint and link of the subgraph will be copied
* into the environment graph. The prefix argument is meant to allow adding multiple copies of the same subgraph with
* Merge a sub-graph into the current environment, considering that the root of the merged graph is attached to the
* root of the environment by a fixed joint and no displacement. Every joint and link of the sub-graph will be copied
* into the environment graph. The prefix argument is meant to allow adding multiple copies of the same sub-graph with
* different names
*/
bool insertSceneGraph(const tesseract_scene_graph::SceneGraph& scene_graph, const std::string& prefix = "");
Expand All @@ -543,11 +543,11 @@ class SceneGraph
* @brief Merge a graph into the current environment
* @param scene_graph Const ref to the graph to be merged (said graph will be copied)
* @param joint The joint that connects current environment with the inserted graph
* @param prefix string Will be prepended to every link and joint of the merged graph
* @param prefix string Will prepend to every link and joint of the merged graph
* @return Return False if any link or joint name collides with current environment, otherwise True
* Merge a subgraph into the current environment. Every joint and link of the subgraph will be copied into the
* environment graph. The prefix argument is meant to allow adding multiple copies of the same subgraph with different
* names
* Merge a sub-graph into the current environment. Every joint and link of the sub-graph will be copied into the
* environment graph. The prefix argument is meant to allow adding multiple copies of the same sub-graph with
* different names
*/
bool insertSceneGraph(const tesseract_scene_graph::SceneGraph& scene_graph,
const tesseract_scene_graph::Joint& joint,
Expand Down
2 changes: 1 addition & 1 deletion tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ KDLTreeData parseSceneGraph(const SceneGraph& scene_graph);

/**
* @brief Convert a portion of a Tesseract SceneGraph into a KDL Tree
* @details This will create a new tree from multple sub tree defined by the provided joint names
* @details This will create a new tree from multiple sub tree defined by the provided joint names
* The values are used to convert non fixed joints that are not listed in joint_names to a
* fixed joint. The first tree found a link is defined attaching world to the base link and all
* other trees are attached to this link by a fixed joint.
Expand Down
106 changes: 77 additions & 29 deletions tesseract_scene_graph/src/graph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,13 +30,34 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <fstream>
#include <queue>
#include <console_bridge/console.h>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/copy.hpp>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_scene_graph/graph.h>
#include <tesseract_common/utils.h>

namespace tesseract_scene_graph
{
using UGraph =
boost::adjacency_list<boost::listS, boost::listS, boost::undirectedS, VertexProperty, EdgeProperty, GraphProperty>;

struct ugraph_vertex_copier
{
ugraph_vertex_copier(const Graph& g1, UGraph& g2)
: vertex_all_map1(get(boost::vertex_all, g1)), vertex_all_map2(get(boost::vertex_all, g2))
{
}

template <typename Vertex1, typename Vertex2>
void operator()(const Vertex1& v1, Vertex2& v2) const
{
boost::put(vertex_all_map2, v2, get(vertex_all_map1, v1));
}
typename boost::property_map<Graph, boost::vertex_all_t>::const_type vertex_all_map1;
mutable typename boost::property_map<UGraph, boost::vertex_all_t>::type vertex_all_map2;
};

SceneGraph::SceneGraph(const std::string& name) : acm_(std::make_shared<AllowedCollisionMatrix>())
{
boost::set_property(static_cast<Graph&>(*this), boost::graph_name, name);
Expand Down Expand Up @@ -612,7 +633,7 @@ std::vector<Joint::ConstPtr> SceneGraph::getInboundJoints(const std::string& lin
std::vector<Joint::ConstPtr> joints;
Vertex vertex = getVertex(link_name);

// Get incomming edges
// Get incoming edges
auto num_in_edges = static_cast<int>(boost::in_degree(vertex, *this));
if (num_in_edges == 0) // The root of the tree will have not incoming edges
return joints;
Expand All @@ -632,7 +653,7 @@ std::vector<Joint::ConstPtr> SceneGraph::getOutboundJoints(const std::string& li
std::vector<Joint::ConstPtr> joints;
Vertex vertex = getVertex(link_name);

// Get incomming edges
// Get incoming edges
auto num_out_edges = static_cast<int>(boost::out_degree(vertex, *this));
if (num_out_edges == 0)
return joints;
Expand Down Expand Up @@ -798,35 +819,62 @@ void SceneGraph::saveDOT(const std::string& path) const

ShortestPath SceneGraph::getShortestPath(const std::string& root, const std::string& tip) const
{
const Graph& graph = *this;
Vertex s = getVertex(root);
// Must copy to undirected graph because order does not matter for creating kinematics chains.

std::map<Vertex, Vertex> predicessor_map;
boost::associative_property_map<std::map<Vertex, Vertex>> prop_predicessor_map(predicessor_map);
// Copy Graph
UGraph graph;

std::map<Vertex, double> distance_map;
boost::associative_property_map<std::map<Vertex, double>> prop_distance_map(distance_map);
std::map<Graph::vertex_descriptor, size_t> index_map;
boost::associative_property_map<std::map<Graph::vertex_descriptor, size_t>> prop_index_map(index_map);

std::map<Vertex, size_t> index_map;
boost::associative_property_map<std::map<Vertex, size_t>> prop_index_map(index_map);
{
int c = 0;
Graph::vertex_iterator i, iend;
for (boost::tie(i, iend) = boost::vertices(*this); i != iend; ++i, ++c)
boost::put(prop_index_map, *i, c);
}

int c = 0;
Graph::vertex_iterator i, iend;
for (boost::tie(i, iend) = boost::vertices(graph); i != iend; ++i, ++c)
boost::put(prop_index_map, *i, c);
ugraph_vertex_copier v_copier(*this, graph);
boost::copy_graph(*this, graph, boost::vertex_index_map(prop_index_map).vertex_copy(v_copier));

// Search Graph
UGraph::vertex_descriptor s_root = getVertex(root);
UGraph::vertex_descriptor s_tip = getVertex(tip);

auto prop_weight_map = boost::get(boost::edge_weight, graph);

std::map<UGraph::vertex_descriptor, UGraph::vertex_descriptor> predicessor_map;
boost::associative_property_map<std::map<UGraph::vertex_descriptor, UGraph::vertex_descriptor>> prop_predicessor_map(
predicessor_map);

std::map<UGraph::vertex_descriptor, double> distance_map;
boost::associative_property_map<std::map<UGraph::vertex_descriptor, double>> prop_distance_map(distance_map);

std::map<Edge, double> weight_map;
boost::associative_property_map<std::map<Edge, double>> prop_weight_map(weight_map);
Graph::edge_iterator j, jend;
for (boost::tie(j, jend) = boost::edges(graph); j != jend; ++j)
boost::put(prop_weight_map, *j, boost::get(boost::edge_weight, graph)[*j]);
std::map<UGraph::vertex_descriptor, size_t> u_index_map;
boost::associative_property_map<std::map<UGraph::vertex_descriptor, size_t>> u_prop_index_map(u_index_map);

{ // Populate index map
int c = 0;
UGraph::vertex_iterator i, iend;
for (boost::tie(i, iend) = boost::vertices(graph); i != iend; ++i, ++c)
{
std::string name = boost::get(boost::vertex_link, graph)[*i]->getName();
if (name == root)
s_root = *i;

if (name == tip)
s_tip = *i;

boost::put(u_prop_index_map, *i, c);
}
}

dijkstra_shortest_paths(graph,
s,
s_root,
prop_predicessor_map,
prop_distance_map,
prop_weight_map,
prop_index_map,
u_prop_index_map,
std::less<>(),
boost::closed_plus<double>(),
(std::numeric_limits<double>::max)(),
Expand All @@ -837,14 +885,14 @@ ShortestPath SceneGraph::getShortestPath(const std::string& root, const std::str
path.links.reserve(predicessor_map.size());
path.joints.reserve(predicessor_map.size());
path.active_joints.reserve(predicessor_map.size());
Vertex v = getVertex(tip); // We want to start at the destination and work our way back to the source
for (Vertex u = predicessor_map[v]; // Start by setting 'u' to the destintaion node's predecessor
u != v; // Keep tracking the path until we get to the source
v = u, u = predicessor_map[v]) // Set the current vertex to the current predecessor, and the predecessor to one
// level up
// We want to start at the destination and work our way back to the source
for (Vertex u = predicessor_map[s_tip]; // Start by setting 'u' to the destination node's predecessor
u != s_tip; // Keep tracking the path until we get to the source
s_tip = u, u = predicessor_map[s_tip]) // Set the current vertex to the current predecessor, and the predecessor
// to one level up
{
path.links.push_back(boost::get(boost::vertex_link, graph)[v]->getName());
const Joint::ConstPtr& joint = boost::get(boost::edge_joint, graph)[boost::edge(u, v, graph).first];
path.links.push_back(boost::get(boost::vertex_link, graph)[s_tip]->getName());
const Joint::ConstPtr& joint = boost::get(boost::edge_joint, graph)[boost::edge(u, s_tip, graph).first];

path.joints.push_back(joint->getName());
if (joint->type != JointType::FIXED && joint->type != JointType::FLOATING)
Expand All @@ -857,7 +905,7 @@ ShortestPath SceneGraph::getShortestPath(const std::string& root, const std::str

#ifndef NDEBUG
CONSOLE_BRIDGE_logDebug("distances and parents:");
Graph::vertex_iterator vi, vend;
UGraph::vertex_iterator vi, vend;
for (boost::tie(vi, vend) = boost::vertices(graph); vi != vend; ++vi)
{
CONSOLE_BRIDGE_logDebug("distance(%s) = %d, parent(%s) = %s",
Expand Down
55 changes: 35 additions & 20 deletions tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,26 +228,41 @@ TEST(TesseractSceneGraphUnit, TesseractSceneGraphUnit) // NOLINT
std::cout << "Is Tree: " << g.isTree() << std::endl;
EXPECT_FALSE(g.isTree());

// Get Shortest Path
ShortestPath path = g.getShortestPath("link_1", "link_4");

// Todo:: Look at using filtered graph for chains and subgraphs
// boost::filtered_graph<

std::cout << path << std::endl;
EXPECT_TRUE(path.links.size() == 4);
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_1") != path.links.end());
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_2") != path.links.end());
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_3") != path.links.end());
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_4") != path.links.end());
EXPECT_TRUE(path.joints.size() == 3);
EXPECT_TRUE(std::find(path.joints.begin(), path.joints.end(), "joint_1") != path.joints.end());
EXPECT_TRUE(std::find(path.joints.begin(), path.joints.end(), "joint_2") != path.joints.end());
EXPECT_TRUE(std::find(path.joints.begin(), path.joints.end(), "joint_3") != path.joints.end());
EXPECT_TRUE(path.active_joints.size() == 1);
EXPECT_TRUE(std::find(path.active_joints.begin(), path.active_joints.end(), "joint_2") != path.active_joints.end());

std::cout << (g.getName().c_str()) << std::endl;
{ // Get Shortest Path
ShortestPath path = g.getShortestPath("link_1", "link_4");

std::cout << path << std::endl;
EXPECT_TRUE(path.links.size() == 3);
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_1") != path.links.end());
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_5") != path.links.end());
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_4") != path.links.end());
EXPECT_TRUE(path.joints.size() == 2);
EXPECT_TRUE(std::find(path.joints.begin(), path.joints.end(), "joint_5") != path.joints.end());
EXPECT_TRUE(std::find(path.joints.begin(), path.joints.end(), "joint_6") != path.joints.end());
EXPECT_TRUE(path.active_joints.size() == 2);
EXPECT_TRUE(std::find(path.active_joints.begin(), path.active_joints.end(), "joint_5") != path.active_joints.end());
EXPECT_TRUE(std::find(path.active_joints.begin(), path.active_joints.end(), "joint_6") != path.active_joints.end());

std::cout << (g.getName().c_str()) << std::endl;
}

{ // Get Shortest Path wit links reversed
ShortestPath path = g.getShortestPath("link_4", "link_1");

std::cout << path << std::endl;
EXPECT_TRUE(path.links.size() == 3);
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_1") != path.links.end());
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_5") != path.links.end());
EXPECT_TRUE(std::find(path.links.begin(), path.links.end(), "link_4") != path.links.end());
EXPECT_TRUE(path.joints.size() == 2);
EXPECT_TRUE(std::find(path.joints.begin(), path.joints.end(), "joint_5") != path.joints.end());
EXPECT_TRUE(std::find(path.joints.begin(), path.joints.end(), "joint_6") != path.joints.end());
EXPECT_TRUE(path.active_joints.size() == 2);
EXPECT_TRUE(std::find(path.active_joints.begin(), path.active_joints.end(), "joint_5") != path.active_joints.end());
EXPECT_TRUE(std::find(path.active_joints.begin(), path.active_joints.end(), "joint_6") != path.active_joints.end());

std::cout << (g.getName().c_str()) << std::endl;
}

// Should throw since this is a directory and not a file
EXPECT_ANY_THROW(g.saveDOT(tesseract_common::getTempPath())); // NOLINT
Expand Down

0 comments on commit 3f1c049

Please sign in to comment.