Make getShortestPath const

Indeed, it does not (and should not) modify the underlying graph.

Author: haudren

tesseract/tesseract_scene_graph/include/tesseract_scene_graph/graph.h

@@ -418,7 +418,7 @@ class SceneGraph : public Graph
    * @param tip The tip link
    * @return The shortest path between the two links
    */
-  Path getShortestPath(const std::string& root, const std::string& tip);
+  Path getShortestPath(const std::string& root, const std::string& tip) const;
 
   /**
    * @brief Get the graph vertex by name

tesseract/tesseract_scene_graph/src/graph.cpp

@@ -560,9 +560,9 @@ void SceneGraph::saveDOT(const std::string& path) const
   dot_file << "}";
 }
 
-SceneGraph::Path SceneGraph::getShortestPath(const std::string& root, const std::string& tip)
+SceneGraph::Path SceneGraph::getShortestPath(const std::string& root, const std::string& tip) const
 {
-  const auto& graph = static_cast<const Graph&>(*this);
+  const Graph& graph = *this;
   Vertex s = getVertex(root);
 
   std::map<Vertex, Vertex> predicessor_map;