From 3f1c0494acbdbe23f6fef373c5e6b2dd6a978224 Mon Sep 17 00:00:00 2001 From: Levi-Armstrong Date: Fri, 22 Oct 2021 08:00:26 -0500 Subject: [PATCH] Update SceneGraph get shortest path to use an undirected graph --- .../include/tesseract_scene_graph/graph.h | 24 ++-- .../tesseract_scene_graph/kdl_parser.h | 2 +- tesseract_scene_graph/src/graph.cpp | 106 +++++++++++++----- .../test/tesseract_scene_graph_unit.cpp | 55 +++++---- 4 files changed, 125 insertions(+), 62 deletions(-) diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/graph.h b/tesseract_scene_graph/include/tesseract_scene_graph/graph.h index 46e51757f2f..778e1f35c23 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/graph.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/graph.h @@ -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; @@ -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); @@ -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 */ @@ -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 = ""); @@ -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, diff --git a/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h b/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h index 806b6ea918e..4db40cdd4d3 100644 --- a/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h +++ b/tesseract_scene_graph/include/tesseract_scene_graph/kdl_parser.h @@ -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. diff --git a/tesseract_scene_graph/src/graph.cpp b/tesseract_scene_graph/src/graph.cpp index 419f140a4af..e57d36a4cb9 100644 --- a/tesseract_scene_graph/src/graph.cpp +++ b/tesseract_scene_graph/src/graph.cpp @@ -30,6 +30,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -37,6 +39,25 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_scene_graph { +using UGraph = + boost::adjacency_list; + +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 + void operator()(const Vertex1& v1, Vertex2& v2) const + { + boost::put(vertex_all_map2, v2, get(vertex_all_map1, v1)); + } + typename boost::property_map::const_type vertex_all_map1; + mutable typename boost::property_map::type vertex_all_map2; +}; + SceneGraph::SceneGraph(const std::string& name) : acm_(std::make_shared()) { boost::set_property(static_cast(*this), boost::graph_name, name); @@ -612,7 +633,7 @@ std::vector SceneGraph::getInboundJoints(const std::string& lin std::vector joints; Vertex vertex = getVertex(link_name); - // Get incomming edges + // Get incoming edges auto num_in_edges = static_cast(boost::in_degree(vertex, *this)); if (num_in_edges == 0) // The root of the tree will have not incoming edges return joints; @@ -632,7 +653,7 @@ std::vector SceneGraph::getOutboundJoints(const std::string& li std::vector joints; Vertex vertex = getVertex(link_name); - // Get incomming edges + // Get incoming edges auto num_out_edges = static_cast(boost::out_degree(vertex, *this)); if (num_out_edges == 0) return joints; @@ -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 predicessor_map; - boost::associative_property_map> prop_predicessor_map(predicessor_map); + // Copy Graph + UGraph graph; - std::map distance_map; - boost::associative_property_map> prop_distance_map(distance_map); + std::map index_map; + boost::associative_property_map> prop_index_map(index_map); - std::map index_map; - boost::associative_property_map> 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 predicessor_map; + boost::associative_property_map> prop_predicessor_map( + predicessor_map); + + std::map distance_map; + boost::associative_property_map> prop_distance_map(distance_map); - std::map weight_map; - boost::associative_property_map> 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 u_index_map; + boost::associative_property_map> 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(), (std::numeric_limits::max)(), @@ -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) @@ -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", diff --git a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp index 90551afd692..ae082c41dd5 100644 --- a/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp +++ b/tesseract_scene_graph/test/tesseract_scene_graph_unit.cpp @@ -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