Merge pull request #215 from jcarpent/devel

Add timings for collision and distance computations

CMakeLists.txt

@@ -81,7 +81,7 @@ EXPORT_BOOST_DEFAULT_OPTIONS()
 IF(WIN32)
   ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS chrono thread date_time serialization)
 ELSE(WIN32)
-  ADD_PROJECT_DEPENDENCY(Boost REQUIRED serialization)
+  ADD_PROJECT_DEPENDENCY(Boost REQUIRED chrono serialization)
 ENDIF(WIN32)
 if (BUILD_PYTHON_INTERFACE)
   FINDPYTHON()
@@ -185,6 +185,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/serialization/OBB.h
   include/hpp/fcl/serialization/RSS.h
   include/hpp/fcl/serialization/OBBRSS.h
+  include/hpp/fcl/timings.h
   )
 
 add_subdirectory(doc)

include/hpp/fcl/collision.h

@@ -44,6 +44,7 @@
 #include <hpp/fcl/collision_object.h>
 #include <hpp/fcl/collision_data.h>
 #include <hpp/fcl/collision_func_matrix.h>
+#include <hpp/fcl/timings.h>
 
 namespace hpp
 {
@@ -110,11 +111,15 @@ public:
     solver.distance_upper_bound = request.distance_upper_bound;
 
     std::size_t res;
-    if (swap_geoms) {
-      res = func(o2, tf2, o1, tf1, &solver, request, result);
-      result.swapObjects();
-    } else {
-      res = func (o1, tf1, o2, tf2, &solver, request, result);
+    {
+      Timer timer;
+      if (swap_geoms) {
+        res = func(o2, tf2, o1, tf1, &solver, request, result);
+        result.swapObjects();
+      } else {
+        res = func (o1, tf1, o2, tf2, &solver, request, result);
+      }
+      result.timings = timer.elapsed();
     }
 
     if (cached) {

include/hpp/fcl/collision_data.h

@@ -46,6 +46,7 @@
 #include <hpp/fcl/collision_object.h>
 #include <hpp/fcl/config.hh>
 #include <hpp/fcl/data_types.h>
+#include <hpp/fcl/timings.h>
 
 namespace hpp
 {
@@ -171,6 +172,9 @@ struct HPP_FCL_DLLAPI QueryResult
   /// @brief stores the last support function vertex index, when relevant.
   support_func_guess_t cached_support_func_guess;
   
+  /// @brief timings for the given request
+  CPUTimes timings;
+  
   QueryResult()
   : cached_gjk_guess(Vec3f::Zero())
   , cached_support_func_guess(support_func_guess_t::Constant(-1))
@@ -338,6 +342,7 @@ public:
     distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
     contacts.clear();
     distance_lower_bound = (std::numeric_limits<FCL_REAL>::max)();
+    timings.clear();
   }
 
   /// @brief reposition Contact objects when fcl inverts them
@@ -483,6 +488,7 @@ public:
     b1 = NONE;
     b2 = NONE;
     nearest_points [0] = nearest_points [1] = normal = nan;
+    timings.clear();
   }
 
   /// @brief whether two DistanceResult are the same or not

include/hpp/fcl/distance.h

@@ -41,6 +41,7 @@
 #include <hpp/fcl/collision_object.h>
 #include <hpp/fcl/collision_data.h>
 #include <hpp/fcl/distance_func_matrix.h>
+#include <hpp/fcl/timings.h>
 
 namespace hpp
 {
@@ -103,14 +104,18 @@ public:
     }
 
     FCL_REAL res;
-    if (swap_geoms) {
-      res = func(o2, tf2, o1, tf1, &solver, request, result);
-      if (request.enable_nearest_points) {
-        std::swap(result.o1, result.o2);
-        result.nearest_points[0].swap(result.nearest_points[1]);
+    {
+      Timer timer;
+      if (swap_geoms) {
+        res = func(o2, tf2, o1, tf1, &solver, request, result);
+        if (request.enable_nearest_points) {
+          std::swap(result.o1, result.o2);
+          result.nearest_points[0].swap(result.nearest_points[1]);
+        }
+      } else {
+        res = func (o1, tf1, o2, tf2, &solver, request, result);
       }
-    } else {
-      res = func (o1, tf1, o2, tf2, &solver, request, result);
+      result.timings = timer.elapsed();
     }
 
     if (cached) {

include/hpp/fcl/timings.h

+//
+// Copyright (c) 2021 INRIA
+//
+
+#ifndef HPP_FCL_TIMINGS_FWD_H
+#define HPP_FCL_TIMINGS_FWD_H
+
+#include <boost/chrono.hpp>
+
+#include "hpp/fcl/fwd.hh"
+
+namespace hpp { namespace fcl {
+
+  struct CPUTimes
+  {
+    FCL_REAL wall;
+    FCL_REAL user;
+    FCL_REAL system;
+    
+    CPUTimes()
+    : wall(0)
+    , user(0)
+    , system(0)
+    {}
+    
+    void clear()
+    {
+      wall = user = system = 0;
+    }
+    
+  };
+
+  namespace internal
+  {
+    inline void get_cpu_times(CPUTimes & current)
+    {
+      using namespace boost::chrono;
+      
+      process_real_cpu_clock::time_point wall = process_real_cpu_clock::now();
+      process_user_cpu_clock::time_point user = process_user_cpu_clock::now();
+      process_system_cpu_clock::time_point system = process_system_cpu_clock::now();
+      
+      current.wall = time_point_cast<nanoseconds>(wall).time_since_epoch().count()*1e-3;
+      current.user = time_point_cast<nanoseconds>(user).time_since_epoch().count()*1e-3;
+      current.system = time_point_cast<nanoseconds>(system).time_since_epoch().count()*1e-3;
+    }
+  }
+
+  ///
+  /// @brief This class mimics the way "boost/timer/timer.hpp" operates while using moder boost::chrono library.
+  ///
+  struct Timer
+  {
+    
+    Timer()
+    {
+      start();
+    }
+    
+    CPUTimes elapsed() const
+    {
+      if(m_is_stopped)
+        return m_times;
+      
+      CPUTimes current;
+      internal::get_cpu_times(current);
+      current.wall -= m_times.wall;
+      current.user -= m_times.user;
+      current.system -= m_times.system;
+      return current;
+    }
+    
+    void start()
+    {
+      m_is_stopped = false;
+      internal::get_cpu_times(m_times);
+    }
+    
+    void stop()
+    {
+      if(m_is_stopped)
+        return;
+      m_is_stopped = true;
+      
+      CPUTimes current;
+      internal::get_cpu_times(current);
+      m_times.wall = (current.wall - m_times.wall);
+      m_times.user = (current.user - m_times.user);
+      m_times.system = (current.system - m_times.system);
+      
+    }
+    
+    void resume()
+    {
+      if(m_is_stopped)
+      {
+        CPUTimes current(m_times);
+        start();
+        m_times.wall   -= current.wall;
+        m_times.user   -= current.user;
+        m_times.system -= current.system;
+      }
+    }
+    
+    bool is_stopped() const
+    {
+      return m_is_stopped;
+    }
+    
+  protected:
+    
+    CPUTimes m_times;
+    bool m_is_stopped;
+  };
+
+}}
+
+#endif // ifndef HPP_FCL_TIMINGS_FWD_H

python/collision.cc

@@ -80,6 +80,16 @@ void exposeCollisionAPI ()
       .export_values()
       ;
   }
+  
+  if(!eigenpy::register_symbolic_link_to_registered_type<CPUTimes>())
+  {
+    class_<CPUTimes>("CPUTimes",no_init)
+    .def_readonly("wall",&CPUTimes::wall,"wall time in micro seconds (us)")
+    .def_readonly("user",&CPUTimes::user,"user time in micro seconds (us)")
+    .def_readonly("system",&CPUTimes::system,"system time in micro seconds (us)")
+    .def("clear",&CPUTimes::clear,arg("self"),"Reset the time values.")
+    ;
+  }
 
   if(!eigenpy::register_symbolic_link_to_registered_type<QueryRequest>())
   {
@@ -149,6 +159,7 @@ void exposeCollisionAPI ()
         doxygen::class_doc<QueryResult>(), no_init)
       .DEF_RW_CLASS_ATTRIB (QueryResult, cached_gjk_guess         )
       .DEF_RW_CLASS_ATTRIB (QueryResult, cached_support_func_guess)
+      .DEF_RW_CLASS_ATTRIB (QueryResult, timings)
       ;
   }
 

src/CMakeLists.txt

@@ -175,12 +175,12 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
 TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
   PUBLIC
   Boost::serialization
+  Boost::chrono
 )
 
 IF(WIN32)
   TARGET_LINK_LIBRARIES(${LIBRARY_NAME}
     INTERFACE
-    Boost::chrono
     Boost::thread
     Boost::date_time
   )

test/benchmark.cpp

@@ -99,7 +99,7 @@ double distance (const std::vector<Transform3f>& tf,
 
   node.enable_statistics = verbose;
 
-  Timer timer;
+  BenchTimer timer;
   timer.start();
 
   for (std::size_t i = 0; i < tf.size(); ++i) {
@@ -125,7 +125,7 @@ double collide (const std::vector<Transform3f>& tf,
 
   node.enable_statistics = verbose;
 
-  Timer timer;
+  BenchTimer timer;
   timer.start();
 
   for (std::size_t i = 0; i < tf.size(); ++i) {

test/profiling.cpp

@@ -96,7 +96,7 @@ void collide(const std::vector<Transform3f>& tf,
              Results& results)
 {
   Transform3f Id;
-  Timer timer;
+  BenchTimer timer;
   for (std::size_t i = 0; i < tf.size(); ++i) {
     timer.start();
     /* int num_contact = */

test/utility.cpp

@@ -11,7 +11,7 @@ namespace hpp
 namespace fcl
 {
 
-Timer::Timer()
+BenchTimer::BenchTimer()
 {
 #ifdef _WIN32
   QueryPerformanceFrequency(&frequency);
@@ -28,12 +28,12 @@ Timer::Timer()
 }
 
 
-Timer::~Timer()
+BenchTimer::~BenchTimer()
 {
 }
 
 
-void Timer::start()
+void BenchTimer::start()
 {
   stopped = 0; // reset stop flag
 #ifdef _WIN32
@@ -44,7 +44,7 @@ void Timer::start()
 }
 
 
-void Timer::stop()
+void BenchTimer::stop()
 {
   stopped = 1; // set timer stopped flag
 
@@ -56,7 +56,7 @@ void Timer::stop()
 }
 
 
-double Timer::getElapsedTimeInMicroSec()
+double BenchTimer::getElapsedTimeInMicroSec()
 {
 #ifdef _WIN32
   if(!stopped)
@@ -76,19 +76,19 @@ double Timer::getElapsedTimeInMicroSec()
 }
 
 
-double Timer::getElapsedTimeInMilliSec()
+double BenchTimer::getElapsedTimeInMilliSec()
 {
   return this->getElapsedTimeInMicroSec() * 0.001;
 }
 
 
-double Timer::getElapsedTimeInSec()
+double BenchTimer::getElapsedTimeInSec()
 {
   return this->getElapsedTimeInMicroSec() * 0.000001;
 }
 
 
-double Timer::getElapsedTime()
+double BenchTimer::getElapsedTime()
 {
   return this->getElapsedTimeInMilliSec();
 }

test/utility.h

@@ -70,11 +70,11 @@ namespace fcl
 typedef shared_ptr<octomap::OcTree> OcTreePtr_t;
 #endif
 
-class Timer
+class BenchTimer
 {
 public:
-  Timer();
-  ~Timer();
+  BenchTimer();
+  ~BenchTimer();
 
   void start();                               ///< start timer
   void stop();                                ///< stop the timer