added missing code changes

started working on refactoring of sensors
new test-cases

nav/dijkstra/DijkstraPath.h

@@ -48,6 +48,8 @@ public:
 	const DijkstraNode<T>& operator [] (const int idx) const	{return *(path[idx]);}
 	size_t size() const											{return path.size();}
 
+	/** get the idx-th element from the starting-node. if this is beyond the end, the last node is returned */
+	const DijkstraNode<T>& getFromStart(const int idx) const	{return (idx >= (int) path.size()) ? (*path.back()) : (*path[idx]);}
 
 	/** NANOFLANN: number of elements in the path */
 	inline int kdtree_get_point_count() const {