diff --git a/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.pro b/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.pro
index a061345c00..cf2012accf 100644
--- a/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.pro
+++ b/pedestrian_routing_benchmarks/pedestrian_routing_benchmarks.pro
@@ -16,4 +16,4 @@ win32* : LIBS *= -lShell32
 
 SOURCES += \
   ../testing/testingmain.cpp \
-  pedestrian_routing_test.cc \
+  pedestrian_routing_test.cpp \
diff --git a/pedestrian_routing_benchmarks/pedestrian_routing_test.cc b/pedestrian_routing_benchmarks/pedestrian_routing_test.cc
deleted file mode 100644
index b14079877e..0000000000
--- a/pedestrian_routing_benchmarks/pedestrian_routing_test.cc
+++ /dev/null
@@ -1,53 +0,0 @@
-#include "../testing/testing.hpp"
-
-#include "../indexer/index.hpp"
-#include "../indexer/classificator_loader.hpp"
-
-#include "../routing/astar_router.hpp"
-#include "../routing/features_road_graph.hpp"
-
-#include "../base/logging.hpp"
-
-#include "../std/string.hpp"
-#include "../std/vector.hpp"
-
-
-void TestTwoPoints(uint32_t featureIdStart, uint32_t segIdStart, uint32_t featureIdFinal, uint32_t segIdFinal)
-{
-  string const kMapName = "UK_England";
-  classificator::Load();
-  Index index;
-  routing::AStarRouter router(&index);
-
-  m2::RectD rect;
-  index.AddMap(kMapName + DATA_FILE_EXTENSION, rect);
-  TEST(index.IsLoaded(kMapName), ());
-  MwmSet::MwmId id = index.GetMwmIdByName(kMapName + DATA_FILE_EXTENSION);
-  TEST_NOT_EQUAL(static_cast<size_t>(-1), id, ());
-
-  router.SetRoadGraph(new routing::FeaturesRoadGraph(&index, id));
-
-  vector<routing::RoadPos> startPos = {{featureIdStart, true, segIdStart}, {featureIdStart, false, segIdStart}};
-  vector<routing::RoadPos> finalPos = {{featureIdFinal, true, segIdFinal}, {featureIdFinal, false, segIdFinal}};
-  router.SetFinalRoadPos(finalPos);
-
-  vector<routing::RoadPos> route;
-  LOG(LINFO, ("Calculating routing..."));
-  router.CalculateRoute(startPos, route);
-  LOG(LINFO, ("Route length:", route.size()));
-}
-
-UNIT_TEST(PedestrianRouting_UK_Long1)
-{
-  TestTwoPoints(59231052, 8, 49334376, 0);
-}
-
-UNIT_TEST(PedestrianRouting_UK_Medium1)
-{
-  TestTwoPoints(3038057, 0, 45899679, 3);
-}
-
-UNIT_TEST(PedestrianRouting_UK_Short1)
-{
-  TestTwoPoints(3038057, 0, 3032688, 3);
-}
diff --git a/pedestrian_routing_benchmarks/pedestrian_routing_test.cpp b/pedestrian_routing_benchmarks/pedestrian_routing_test.cpp
new file mode 100644
index 0000000000..960c91eb93
--- /dev/null
+++ b/pedestrian_routing_benchmarks/pedestrian_routing_test.cpp
@@ -0,0 +1,69 @@
+#include "../testing/testing.hpp"
+
+#include "../indexer/index.hpp"
+#include "../indexer/classificator_loader.hpp"
+
+#include "../routing/astar_router.hpp"
+#include "../routing/features_road_graph.hpp"
+
+#include "../base/logging.hpp"
+#include "../base/timer.hpp"
+
+#include "../std/string.hpp"
+#include "../std/utility.hpp"
+#include "../std/vector.hpp"
+
+pair<m2::PointD, m2::PointD> GetPointsAroundSeg(Index & index, MwmSet::MwmId id, uint32_t featureId,
+                                                uint32_t segId)
+{
+  FeatureType ft;
+  Index::FeaturesLoaderGuard loader(index, id);
+  loader.GetFeature(featureId, ft);
+  ft.ParseGeometry(FeatureType::BEST_GEOMETRY);
+  CHECK_LESS(segId + 1, ft.GetPointsCount(),
+             ("Wrong segment id:", segId, "for a feature with", ft.GetPointsCount(), "points"));
+  return make_pair(ft.GetPoint(segId), ft.GetPoint(segId + 1));
+}
+
+void TestTwoPoints(uint32_t featureIdStart, uint32_t segIdStart, uint32_t featureIdFinal,
+                   uint32_t segIdFinal)
+{
+  string const kMapName = "UK_England";
+  classificator::Load();
+  Index index;
+  routing::AStarRouter router(&index);
+
+  m2::RectD rect;
+  index.AddMap(kMapName + DATA_FILE_EXTENSION, rect);
+  TEST(index.IsLoaded(kMapName), ());
+  MwmSet::MwmId id = index.GetMwmIdByName(kMapName + DATA_FILE_EXTENSION);
+  TEST_NOT_EQUAL(static_cast<size_t>(-1), id, ());
+
+  router.SetRoadGraph(new routing::FeaturesRoadGraph(&index, id));
+
+  pair<m2::PointD, m2::PointD> startBounds =
+      GetPointsAroundSeg(index, id, featureIdStart, segIdStart);
+
+  vector<routing::RoadPos> startPos = {{featureIdStart, true, segIdStart, startBounds.second},
+                                       {featureIdStart, false, segIdStart, startBounds.first}};
+
+  pair<m2::PointD, m2::PointD> finalBounds =
+      GetPointsAroundSeg(index, id, featureIdStart, segIdStart);
+  vector<routing::RoadPos> finalPos = {{featureIdFinal, true, segIdFinal, finalBounds.second},
+                                       {featureIdFinal, false, segIdFinal, finalBounds.first}};
+  router.SetFinalRoadPos(finalPos);
+
+  vector<routing::RoadPos> route;
+
+  my::Timer timer;
+  LOG(LINFO, ("Calculating routing..."));
+  router.CalculateRoute(startPos, route);
+  LOG(LINFO, ("Route length:", route.size()));
+  LOG(LINFO, ("Elapsed:", timer.ElapsedSeconds(), "seconds"));
+}
+
+UNIT_TEST(PedestrianRouting_UK_Long1) { TestTwoPoints(59231052, 8, 49334376, 0); }
+
+UNIT_TEST(PedestrianRouting_UK_Medium1) { TestTwoPoints(3038057, 0, 45899679, 3); }
+
+UNIT_TEST(PedestrianRouting_UK_Short1) { TestTwoPoints(3038057, 0, 3032688, 3); }
diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp
index 171dc48622..0119ef0dc3 100644
--- a/routing/astar_router.cpp
+++ b/routing/astar_router.cpp
@@ -1,7 +1,6 @@
 #include "astar_router.hpp"
 
 #include "../base/logging.hpp"
-#include "../base/timer.hpp"
 #include "../geometry/distance_on_sphere.hpp"
 #include "../indexer/mercator.hpp"
 
@@ -12,9 +11,10 @@ static double const MAX_SPEED = 36.11111111111; // m/s
 
 void AStarRouter::SetFinalRoadPos(vector<RoadPos> const & finalPos)
 {
-  LOG(LINFO, ("AStarRouter::SetFinalRoadPos() call"));
+#if defined(DEBUG)
   for (auto const & roadPos : finalPos)
-    LOG(LINFO, ("AStarRouter::SetFinalRoadPos(): finalPos:", DebugPrint(roadPos)));
+    LOG(LDEBUG, ("AStarRouter::SetFinalRoadPos(): finalPos:", roadPos));
+#endif  // defined(DEBUG)
 
   ASSERT_GREATER(finalPos.size(), 0, ());
   m_entries.clear();
@@ -30,10 +30,10 @@ void AStarRouter::SetFinalRoadPos(vector<RoadPos> const & finalPos)
 
 void AStarRouter::CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPos> & route)
 {
-  my::Timer timer;
-  LOG(LINFO, ("AStarRouter::CalculateRoute() call start"));
+#if defined(DEBUG)
   for (auto const & roadPos : startPos)
-    LOG(LINFO, ("AStarRouter::CalculateRoute(): startPos:", DebugPrint(roadPos)));
+    LOG(LDEBUG, ("AStarRouter::CalculateRoute(): startPos:", roadPos));
+#endif // defined(DEBUG)
 
   route.clear();
   set<RoadPos> startSet(startPos.begin(), startPos.end());
@@ -55,7 +55,6 @@ void AStarRouter::CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPo
     if (bStartFeature && startSet.find(current->GetPos()) != startSet.end())
     {
       ReconstructRoute(current, route);
-      LOG(LINFO, ("AStarRouter::CalculateRoute() call finish: elapsed:", timer.ElapsedSeconds()));
       return;
     }
 
@@ -86,7 +85,6 @@ void AStarRouter::CalculateRoute(vector<RoadPos> const & startPos, vector<RoadPo
   }
 
   LOG(LDEBUG, ("No route found!"));
-  LOG(LINFO, ("AStarRouter::CalculateRoute() call finish: elapsed:", timer.ElapsedSeconds()));
   // Route not found.
 }
 
diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp
index f7ced78756..91ffe08acf 100644
--- a/routing/road_graph.cpp
+++ b/routing/road_graph.cpp
@@ -18,7 +18,7 @@ string DebugPrint(RoadPos const & r)
 {
   ostringstream ss;
   ss << "{ featureId: " << r.GetFeatureId() << ", isForward: " << r.IsForward()
-     << ", segId:" << r.m_segId << "}";
+     << ", segId:" << r.m_segId << ", segEndpoint:" << DebugPrint(r.m_segEndpoint) << "}";
   return ss.str();
 }