Merge pull request #30 from clearpathrobotics/add_timing_to_state

Add timing to state

Author: Enrique Fernández Perdomo

diff_drive_controller/CMakeLists.txt

@@ -16,7 +16,9 @@ find_package(catkin REQUIRED
     trajectory_msgs
     roslint)
 
-find_package(Boost REQUIRED)
+find_package(Boost REQUIRED
+  COMPONENTS
+    timer)
 
 find_package(Eigen REQUIRED)
 
@@ -54,7 +56,7 @@ roslint_cpp()
 
 add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp)
 # Note that the entry for ${Ceres_LIBRARIES} was removed as we only used headers from that package
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
 
 install(TARGETS ${PROJECT_NAME}

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h

@@ -57,6 +57,8 @@
 #include <diff_drive_controller/speed_limiter.h>
 #include <diff_drive_controller/DiffDriveControllerConfig.h>
 
+#include <boost/timer/timer.hpp>
+
 #include <vector>
 #include <string>
 
@@ -185,6 +187,9 @@ namespace diff_drive_controller
     typedef dynamic_reconfigure::Server<DiffDriveControllerConfig> ReconfigureServer;
     boost::shared_ptr<ReconfigureServer> cfg_server_;
 
+    /// Timing related:
+    boost::timer::cpu_timer cpu_timer_;
+
     struct DynamicParams
     {
       bool pose_from_joint_position;

diff_drive_controller/msg/DiffDriveControllerState.msg

@@ -31,3 +31,18 @@ trajectory_msgs/JointTrajectoryPoint error_estimated_side_average
 float64 control_period_desired
 float64 control_period_actual
 float64 control_period_error
+
+# Control time (of the actual code):
+# Note that the resolution of the wall time is approx. 0.5us, but the user and
+# system is only 10ms. See:
+# http://www.boost.org/doc/libs/1_48_0/libs/timer/doc/cpu_timers.html#Resolution
+#
+# For this reason, and given that the control time is much less than 10ms,
+# the user and system time aren't accurate; note that we can't take an average
+# of multiple cycles because we still have to call stop at the end of the cycle,
+# so the internal count of the timer would still be wrong.
+# Consequently, ignore the user and system time, unless the wall time becomes
+# greater than 10ms.
+float64 control_time_wall
+float64 control_time_user
+float64 control_time_system

diff_drive_controller/src/diff_drive_controller.cpp

@@ -466,6 +466,12 @@ namespace diff_drive_controller
 
   void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
   {
+    // Start/Resume CPU timer to measure the control time:
+    if (publish_state_)
+    {
+      cpu_timer_.start();
+    }
+
     // UPDATE DYNAMIC PARAMS
     // Retreive dynamic params:
     DynamicParams dynamic_params = *(dynamic_params_.readFromRT());
@@ -818,6 +824,7 @@ namespace diff_drive_controller
             state_pub_->msg_.desired.effort[i] - state_pub_->msg_.actual_estimated_side_average.effort[i];
         }
 
+        // Set time from start:
         state_pub_->msg_.desired.time_from_start = ros::Duration(dt);
         state_pub_->msg_.actual.time_from_start = ros::Duration(control_period);
         state_pub_->msg_.error.time_from_start = state_pub_->msg_.actual.time_from_start;
@@ -831,10 +838,18 @@ namespace diff_drive_controller
         state_pub_->msg_.actual_estimated_side_average.time_from_start = state_pub_->msg_.actual.time_from_start;
         state_pub_->msg_.error_estimated_side_average.time_from_start = state_pub_->msg_.actual_estimated_side_average.time_from_start;
 
+        // Set control period (update method):
         state_pub_->msg_.control_period_desired = control_period_desired_;
         state_pub_->msg_.control_period_actual  = period.toSec();
         state_pub_->msg_.control_period_error   = state_pub_->msg_.control_period_desired - state_pub_->msg_.control_period_actual;
 
+        // Set control wall, user and system time:
+        boost::timer::cpu_times control_times = cpu_timer_.elapsed();
+
+        state_pub_->msg_.control_time_wall   = 1e-9 * control_times.wall;
+        state_pub_->msg_.control_time_user   = 1e-9 * control_times.user;
+        state_pub_->msg_.control_time_system = 1e-9 * control_times.system;
+
         state_pub_->unlockAndPublish();
       }