Skip to content

Commit

Permalink
use ros_steadytime
Browse files Browse the repository at this point in the history
which depends on ros/roscpp_core#73
and assumes that it will be available with rostime 0.6.9
  • Loading branch information
flixr committed Feb 2, 2018
1 parent 1b55ad7 commit 679efe9
Show file tree
Hide file tree
Showing 3 changed files with 9 additions and 42 deletions.
4 changes: 2 additions & 2 deletions utilities/xmlrpcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,13 @@ if(NOT WIN32)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
endif()

find_package(catkin REQUIRED COMPONENTS cpp_common)
find_package(catkin REQUIRED COMPONENTS cpp_common rostime)

# The CFG_EXTRAS is only for compatibility, to be removed in Lunar.
catkin_package(
INCLUDE_DIRS include
LIBRARIES xmlrpcpp
CATKIN_DEPENDS cpp_common
CATKIN_DEPENDS cpp_common rostime
CFG_EXTRAS xmlrpcpp-extras.cmake
)

Expand Down
2 changes: 2 additions & 0 deletions utilities/xmlrpcpp/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,10 @@
<buildtool_depend>catkin</buildtool_depend>

<build_depend>cpp_common</build_depend>
<build_depend version_gte="0.6.9">rostime</build_depend>

<run_depend>cpp_common</run_depend>
<run_depend version_gte="0.6.9">rostime</run_depend>

<export>
<rosdoc external="http://xmlrpcpp.sourceforge.net/doc/hierarchy.html"/>
Expand Down
45 changes: 5 additions & 40 deletions utilities/xmlrpcpp/src/XmlRpcDispatch.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
#include "xmlrpcpp/XmlRpcSource.h"
#include "xmlrpcpp/XmlRpcUtil.h"

#include "ros/time.h"

#include <math.h>
#include <errno.h>
#include <sys/timeb.h>
Expand Down Expand Up @@ -222,47 +224,10 @@ XmlRpcDispatch::getTime()
return ((double) tbuff.time + ((double)tbuff.millitm / 1000.0) +
((double) tbuff.timezone * 60));
#else
/*
* copied from ros_steadytime in rostime/src/time.cpp
*/
#ifndef WIN32
timespec ts;
#if defined(__APPLE__)
// On macOS use clock_get_time.
clock_serv_t cclock;
mach_timespec_t mts;
host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &cclock);
clock_get_time(cclock, &mts);
mach_port_deallocate(mach_task_self(), cclock);
ts.tv_sec = mts.tv_sec;
ts.tv_nsec = mts.tv_nsec;
#else // defined(__APPLE__)
// Otherwise use clock_gettime.
clock_gettime(CLOCK_MONOTONIC, &ts);
#endif // defined(__APPLE__)
return (ts.tv_sec + ts.tv_nsec / 1000000000.0);
#else /* WIN32 */
static LARGE_INTEGER cpu_frequency, performance_count;
// These should not ever fail since XP is already end of life:
// From https://msdn.microsoft.com/en-us/library/windows/desktop/ms644905(v=vs.85).aspx and
// https://msdn.microsoft.com/en-us/library/windows/desktop/ms644904(v=vs.85).aspx:
// "On systems that run Windows XP or later, the function will always succeed and will
// thus never return zero."
QueryPerformanceFrequency(&cpu_frequency);
if (cpu_frequency.QuadPart == 0) {
throw NoHighPerformanceTimersException();
}
QueryPerformanceCounter(&performance_count);
double steady_time = performance_count.QuadPart / (double) cpu_frequency.QuadPart;
int64_t steady_sec = floor(steady_time);
int64_t steady_nsec = boost::math::round((steady_time - steady_sec) * 1e9);

// Throws an exception if we go out of 32-bit range
normalizeSecNSecUnsigned(steady_sec, steady_nsec);

return (steady_sec + steady_nsec / 1000000000.0);
#endif /* WIN32 */
uint32_t sec, nsec;

ros_steadytime(sec, nsec);
return ((double)sec + (double)nsec / 1e9);
#endif /* USE_FTIME */
}

Expand Down

0 comments on commit 679efe9

Please sign in to comment.