pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
thirdparty/ros/roscpp_core/rostime/CHANGELOG.rst vendored Normal file
View File

@@ -0,0 +1,145 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rostime
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.7.2 (2020-05-28)
------------------
* [Windows] Using C++11 std::chrono for ros_walltime & ros_steadytime (`#121 <https://github.com/ros/roscpp_core/issues/121>`_)
0.7.1 (2020-01-25)
------------------
* only depend on the boost components needed (`#117 <https://github.com/ros/roscpp_core/issues/117>`_)
0.7.0 (2020-01-24)
------------------
* rostime: remove empty destructor of DurationBase (`#104 <https://github.com/ros/roscpp_core/issues/104>`_)
* various code cleanup (`#116 <https://github.com/ros/roscpp_core/issues/116>`_)
* Bump CMake version to avoid CMP0048 warning (`#115 <https://github.com/ros/roscpp_core/issues/115>`_)
0.6.13 (2019-10-03)
-------------------
* use _WIN32 for platform detection (`#110 <https://github.com/ros/roscpp_core/issues/110>`_)
* Clarified documentation for time validity (`#109 <https://github.com/ros/roscpp_core/issues/109>`_)
* rostime: replace c-style casts with static_casts (`#106 <https://github.com/ros/roscpp_core/issues/106>`_)
0.6.12 (2019-03-04)
-------------------
* use std::numeric_limits instead of * _MAX macros for range checking (`#103 <https://github.com/ros/roscpp_core/issues/103>`_)
* use std::this_thread::sleep_for instead of WaitableTimer (`#101 <https://github.com/ros/roscpp_core/issues/101>`_)
* include windows.h in time.cpp (`#100 <https://github.com/ros/roscpp_core/issues/100>`_)
* fix duration bug and add tests. (`#98 <https://github.com/ros/roscpp_core/issues/98>`_)
* fix for Duration::fromSec() which had rounding issues (`#93 <https://github.com/ros/roscpp_core/issues/93>`_)
* fix bug in HAVE_CXXABI_H compiler check (`#89 <https://github.com/ros/roscpp_core/issues/89>`_)
* add ROSTIME_DECL storage-class attribute (`#90 <https://github.com/ros/roscpp_core/issues/90>`_)
0.6.11 (2018-06-06)
-------------------
* argument to boost microseconds must be integral for Boost 1.67 and newer compatibility (`#79 <https://github.com/ros/roscpp_core/issues/79>`_)
* remove empty destructor of TimeBase (which makes TimeBase, Time and WallTime trivially copyable) (`#82 <https://github.com/ros/roscpp_core/issues/82>`_)
0.6.10 (2018-05-01)
-------------------
* fix conversion of Duration on macOS (`#78 <https://github.com/ros/roscpp_core/issues/78>`_)
0.6.9 (2018-02-02)
------------------
* expose ros_walltime and ros_steadytime (`#73 <https://github.com/ros/roscpp_core/issues/73>`_)
0.6.8 (2018-01-26)
------------------
0.6.7 (2017-11-03)
------------------
0.6.6 (2017-10-25)
------------------
* only use Apple features on Apple (`#68 <https://github.com/ros/roscpp_core/issues/68>`_)
* remove exception specification (`#64 <https://github.com/ros/roscpp_core/issues/64>`_)
0.6.5 (2017-07-27)
------------------
* add additional checks for valid time values (`#62 <https://github.com/ros/roscpp_core/pull/62>`_)
* fix overflow bugs in Time and Duration (`#61 <https://github.com/ros/roscpp_core/pull/61>`_, `#63 <https://github.com/ros/roscpp_core/pull/63>`_)
0.6.4 (2017-06-06)
------------------
* add logic to support steady time on macOS (regression of 0.6.3) (`#59 <https://github.com/ros/roscpp_core/pull/59>`_)
0.6.3 (2017-05-15)
------------------
* add SteadyTime (`#57 <https://github.com/ros/roscpp_core/issues/57>`_)
0.6.2 (2017-02-14)
------------------
0.6.1 (2016-09-02)
------------------
* fix rounding errors leading to invalid stored data in ros::TimeBase (`#48 <https://github.com/ros/roscpp_core/issues/48>`_)
0.6.0 (2016-03-17)
------------------
* change Duration:sleep return semantic (`#47 <https://github.com/ros/roscpp_core/pull/47>`_)
0.5.7 (2016-03-09)
------------------
* Adjust return value of sleep() function (`#45 <https://github.com/ros/roscpp_core/pull/45>`_)
* fix WallRate(Duration) constructor (`#40 <https://github.com/ros/roscpp_core/pull/40>`_)
0.5.6 (2015-05-20)
------------------
0.5.5 (2014-12-22)
------------------
* move implementation of Duration(Rate) constructor (`#30 <https://github.com/ros/roscpp_core/issues/30>`_)
* fix Duration initialization from seconds for negative values (`#29 <https://github.com/ros/roscpp_core/pull/29>`_)
0.5.4 (2014-07-23)
------------------
* fix Rate initialized by Duration (`#26 <https://github.com/ros/roscpp_core/issues/26>`_)
0.5.3 (2014-06-28)
------------------
0.5.2 (2014-06-27)
------------------
0.5.1 (2014-06-24)
------------------
0.5.0 (2014-02-19)
------------------
0.4.2 (2014-02-11)
------------------
0.4.1 (2014-02-11)
------------------
0.4.0 (2014-01-29)
------------------
0.3.17 (2014-01-07)
-------------------
* fix boost include dir
0.3.16 (2013-07-14)
-------------------
* support for CATKIN_ENABLE_TESTING
0.3.15 (2013-06-06)
-------------------
* fix install destination for dll's under Windows
0.3.14 (2013-03-21)
-------------------
0.3.13 (2013-03-08)
-------------------
0.3.12 (2013-01-13)
-------------------
* improve string output of negative durations (`#3309 <https://github.com/ros/roscpp_core/issues/3309>`_)
0.3.11 (2012-12-21)
-------------------
* first public release for Groovy

44
thirdparty/ros/roscpp_core/rostime/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,44 @@
cmake_minimum_required(VERSION 3.0.2)
project(rostime)
find_package(catkin REQUIRED COMPONENTS cpp_common)
find_package(Boost REQUIRED COMPONENTS date_time system thread)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS cpp_common
DEPENDS Boost
)
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
add_library(rostime
src/duration.cpp
src/rate.cpp
src/time.cpp)
target_link_libraries(rostime ${Boost_LIBRARIES})
if(NOT APPLE)
target_link_libraries(rostime ${RT_LIBRARY})
endif()
install(TARGETS rostime
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(DIRECTORY include/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h")
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-test_duration test/duration.cpp)
if(TARGET ${PROJECT_NAME}-test_duration)
target_link_libraries(${PROJECT_NAME}-test_duration ${catkin_LIBRARIES} rostime)
endif()
catkin_add_gtest(${PROJECT_NAME}-test_time test/time.cpp)
if(TARGET ${PROJECT_NAME}-test_time)
set_property(TARGET ${PROJECT_NAME}-test_time APPEND_STRING PROPERTY COMPILE_FLAGS "-std=c++11")
target_link_libraries(${PROJECT_NAME}-test_time ${catkin_LIBRARIES} rostime)
endif()
endif()

160
thirdparty/ros/roscpp_core/rostime/include/ros/duration.h vendored Normal file
View File

@@ -0,0 +1,160 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROS_DURATION_H
#define ROS_DURATION_H
/*********************************************************************
** Pragmas
*********************************************************************/
#ifdef _MSC_VER
// Rostime has some magic interface that doesn't directly include
// its implementation, this just disbales those warnings.
#pragma warning(disable: 4244)
#pragma warning(disable: 4661)
#endif
#include <iostream>
#include <math.h>
#include <stdexcept>
#include <climits>
#include <stdint.h>
#include "rostime_decl.h"
namespace boost {
namespace posix_time {
class time_duration;
}
}
namespace ros
{
ROSTIME_DECL void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec);
ROSTIME_DECL void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec);
/**
* \brief Base class for Duration implementations. Provides storage, common functions and operator overloads.
* This should not need to be used directly.
*/
template <class T>
class DurationBase
{
public:
int32_t sec, nsec;
DurationBase() : sec(0), nsec(0) { }
DurationBase(int32_t _sec, int32_t _nsec);
explicit DurationBase(double t){fromSec(t);};
T operator+(const T &rhs) const;
T operator-(const T &rhs) const;
T operator-() const;
T operator*(double scale) const;
T& operator+=(const T &rhs);
T& operator-=(const T &rhs);
T& operator*=(double scale);
bool operator==(const T &rhs) const;
inline bool operator!=(const T &rhs) const { return !(*static_cast<const T*>(this) == rhs); }
bool operator>(const T &rhs) const;
bool operator<(const T &rhs) const;
bool operator>=(const T &rhs) const;
bool operator<=(const T &rhs) const;
double toSec() const { return static_cast<double>(sec) + 1e-9*static_cast<double>(nsec); };
int64_t toNSec() const {return static_cast<int64_t>(sec)*1000000000ll + static_cast<int64_t>(nsec); };
T& fromSec(double t);
T& fromNSec(int64_t t);
bool isZero() const;
boost::posix_time::time_duration toBoost() const;
};
class Rate;
/**
* \brief Duration representation for use with the Time class.
*
* ros::DurationBase provides most of its functionality.
*/
class ROSTIME_DECL Duration : public DurationBase<Duration>
{
public:
Duration()
: DurationBase<Duration>()
{ }
Duration(int32_t _sec, int32_t _nsec)
: DurationBase<Duration>(_sec, _nsec)
{}
explicit Duration(double t) { fromSec(t); }
explicit Duration(const Rate&);
/**
* \brief sleep for the amount of time specified by this Duration. If a signal interrupts the sleep, resleeps for the time remaining.
* @return True if the desired sleep duration was met, false otherwise.
*/
bool sleep() const;
};
extern ROSTIME_DECL const Duration DURATION_MAX;
extern ROSTIME_DECL const Duration DURATION_MIN;
/**
* \brief Duration representation for use with the WallTime class.
*
* ros::DurationBase provides most of its functionality.
*/
class ROSTIME_DECL WallDuration : public DurationBase<WallDuration>
{
public:
WallDuration()
: DurationBase<WallDuration>()
{ }
WallDuration(int32_t _sec, int32_t _nsec)
: DurationBase<WallDuration>(_sec, _nsec)
{}
explicit WallDuration(double t) { fromSec(t); }
explicit WallDuration(const Rate&);
/**
* \brief sleep for the amount of time specified by this Duration. If a signal interrupts the sleep, resleeps for the time remaining.
* @return True if the desired sleep duration was met, false otherwise.
*/
bool sleep() const;
};
ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const Duration &rhs);
ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const WallDuration &rhs);
}
#endif // ROS_DURATION_H

193
thirdparty/ros/roscpp_core/rostime/include/ros/impl/duration.h vendored Normal file
View File

@@ -0,0 +1,193 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSTIME_IMPL_DURATION_H_INCLUDED
#define ROSTIME_IMPL_DURATION_H_INCLUDED
#include <ros/duration.h>
#include <ros/rate.h>
#include <boost/date_time/posix_time/posix_time_types.hpp>
#include <boost/math/special_functions/round.hpp>
namespace ros {
//
// DurationBase template member function implementation
//
template<class T>
DurationBase<T>::DurationBase(int32_t _sec, int32_t _nsec)
: sec(_sec), nsec(_nsec)
{
normalizeSecNSecSigned(sec, nsec);
}
template<class T>
T& DurationBase<T>::fromSec(double d)
{
int64_t sec64 = static_cast<int64_t>(floor(d));
if (sec64 < std::numeric_limits<int32_t>::min() || sec64 > std::numeric_limits<int32_t>::max())
throw std::runtime_error("Duration is out of dual 32-bit range");
sec = static_cast<int32_t>(sec64);
nsec = static_cast<int32_t>(boost::math::round((d - sec) * 1e9));
int32_t rollover = nsec / 1000000000ul;
sec += rollover;
nsec %= 1000000000ul;
return *static_cast<T*>(this);
}
template<class T>
T& DurationBase<T>::fromNSec(int64_t t)
{
int64_t sec64 = t / 1000000000LL;
if (sec64 < std::numeric_limits<int32_t>::min() || sec64 > std::numeric_limits<int32_t>::max())
throw std::runtime_error("Duration is out of dual 32-bit range");
sec = static_cast<int32_t>(sec64);
nsec = static_cast<int32_t>(t % 1000000000LL);
normalizeSecNSecSigned(sec, nsec);
return *static_cast<T*>(this);
}
template<class T>
T DurationBase<T>::operator+(const T &rhs) const
{
T t;
return t.fromNSec(toNSec() + rhs.toNSec());
}
template<class T>
T DurationBase<T>::operator*(double scale) const
{
return T(toSec() * scale);
}
template<class T>
T DurationBase<T>::operator-(const T &rhs) const
{
T t;
return t.fromNSec(toNSec() - rhs.toNSec());
}
template<class T>
T DurationBase<T>::operator-() const
{
T t;
return t.fromNSec(-toNSec());
}
template<class T>
T& DurationBase<T>::operator+=(const T &rhs)
{
*this = *this + rhs;
return *static_cast<T*>(this);
}
template<class T>
T& DurationBase<T>::operator-=(const T &rhs)
{
*this += (-rhs);
return *static_cast<T*>(this);
}
template<class T>
T& DurationBase<T>::operator*=(double scale)
{
fromSec(toSec() * scale);
return *static_cast<T*>(this);
}
template<class T>
bool DurationBase<T>::operator<(const T &rhs) const
{
if (sec < rhs.sec)
return true;
else if (sec == rhs.sec && nsec < rhs.nsec)
return true;
return false;
}
template<class T>
bool DurationBase<T>::operator>(const T &rhs) const
{
if (sec > rhs.sec)
return true;
else if (sec == rhs.sec && nsec > rhs.nsec)
return true;
return false;
}
template<class T>
bool DurationBase<T>::operator<=(const T &rhs) const
{
if (sec < rhs.sec)
return true;
else if (sec == rhs.sec && nsec <= rhs.nsec)
return true;
return false;
}
template<class T>
bool DurationBase<T>::operator>=(const T &rhs) const
{
if (sec > rhs.sec)
return true;
else if (sec == rhs.sec && nsec >= rhs.nsec)
return true;
return false;
}
template<class T>
bool DurationBase<T>::operator==(const T &rhs) const
{
return sec == rhs.sec && nsec == rhs.nsec;
}
template<class T>
bool DurationBase<T>::isZero() const
{
return sec == 0 && nsec == 0;
}
template <class T>
boost::posix_time::time_duration
DurationBase<T>::toBoost() const
{
namespace bt = boost::posix_time;
#if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
return bt::seconds(sec) + bt::nanoseconds(nsec);
#else
return bt::seconds(sec) + bt::microseconds(nsec/1000);
#endif
}
}
#endif

188
thirdparty/ros/roscpp_core/rostime/include/ros/impl/time.h vendored Normal file
View File

@@ -0,0 +1,188 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROS_TIME_IMPL_H_INCLUDED
#define ROS_TIME_IMPL_H_INCLUDED
/*********************************************************************
** Headers
*********************************************************************/
#include <ros/platform.h>
#include <iostream>
#include <cmath>
#include <ros/exception.h>
#include <ros/time.h>
#include <boost/date_time/posix_time/posix_time.hpp>
/*********************************************************************
** Cross Platform Headers
*********************************************************************/
#if defined(_WIN32)
#include <sys/timeb.h>
#else
#include <sys/time.h>
#endif
namespace ros
{
template<class T, class D>
T& TimeBase<T, D>::fromNSec(uint64_t t)
{
uint64_t sec64 = 0;
uint64_t nsec64 = t;
normalizeSecNSec(sec64, nsec64);
sec = static_cast<uint32_t>(sec64);
nsec = static_cast<uint32_t>(nsec64);
return *static_cast<T*>(this);
}
template<class T, class D>
T& TimeBase<T, D>::fromSec(double t) {
int64_t sec64 = static_cast<int64_t>(floor(t));
if (sec64 < 0 || sec64 > std::numeric_limits<uint32_t>::max())
throw std::runtime_error("Time is out of dual 32-bit range");
sec = static_cast<uint32_t>(sec64);
nsec = static_cast<uint32_t>(boost::math::round((t-sec) * 1e9));
// avoid rounding errors
sec += (nsec / 1000000000ul);
nsec %= 1000000000ul;
return *static_cast<T*>(this);
}
template<class T, class D>
D TimeBase<T, D>::operator-(const T &rhs) const
{
D d;
return d.fromNSec(toNSec() - rhs.toNSec());
}
template<class T, class D>
T TimeBase<T, D>::operator-(const D &rhs) const
{
return *static_cast<const T*>(this) + ( -rhs);
}
template<class T, class D>
T TimeBase<T, D>::operator+(const D &rhs) const
{
int64_t sec_sum = static_cast<uint64_t>(sec) + static_cast<uint64_t>(rhs.sec);
int64_t nsec_sum = static_cast<uint64_t>(nsec) + static_cast<uint64_t>(rhs.nsec);
// Throws an exception if we go out of 32-bit range
normalizeSecNSecUnsigned(sec_sum, nsec_sum);
// now, it's safe to downcast back to uint32 bits
return T(static_cast<uint32_t>(sec_sum), static_cast<uint32_t>(nsec_sum));
}
template<class T, class D>
T& TimeBase<T, D>::operator+=(const D &rhs)
{
*this = *this + rhs;
return *static_cast<T*>(this);
}
template<class T, class D>
T& TimeBase<T, D>::operator-=(const D &rhs)
{
*this += (-rhs);
return *static_cast<T*>(this);
}
template<class T, class D>
bool TimeBase<T, D>::operator==(const T &rhs) const
{
return sec == rhs.sec && nsec == rhs.nsec;
}
template<class T, class D>
bool TimeBase<T, D>::operator<(const T &rhs) const
{
if (sec < rhs.sec)
return true;
else if (sec == rhs.sec && nsec < rhs.nsec)
return true;
return false;
}
template<class T, class D>
bool TimeBase<T, D>::operator>(const T &rhs) const
{
if (sec > rhs.sec)
return true;
else if (sec == rhs.sec && nsec > rhs.nsec)
return true;
return false;
}
template<class T, class D>
bool TimeBase<T, D>::operator<=(const T &rhs) const
{
if (sec < rhs.sec)
return true;
else if (sec == rhs.sec && nsec <= rhs.nsec)
return true;
return false;
}
template<class T, class D>
bool TimeBase<T, D>::operator>=(const T &rhs) const
{
if (sec > rhs.sec)
return true;
else if (sec == rhs.sec && nsec >= rhs.nsec)
return true;
return false;
}
template<class T, class D>
boost::posix_time::ptime
TimeBase<T, D>::toBoost() const
{
namespace pt = boost::posix_time;
#if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
return pt::from_time_t(sec) + pt::nanoseconds(nsec);
#else
return pt::from_time_t(sec) + pt::microseconds(nsec/1000);
#endif
}
}
#endif // ROS_IMPL_TIME_H_INCLUDED

131
thirdparty/ros/roscpp_core/rostime/include/ros/rate.h vendored Normal file
View File

@@ -0,0 +1,131 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef ROSLIB_RATE_H
#define ROSLIB_RATE_H
#include "ros/time.h"
#include "rostime_decl.h"
namespace ros
{
class Duration;
/**
* @class Rate
* @brief Class to help run loops at a desired frequency
*/
class ROSTIME_DECL Rate
{
public:
/**
* @brief Constructor, creates a Rate
* @param frequency The desired rate to run at in Hz
*/
Rate(double frequency);
explicit Rate(const Duration&);
/**
* @brief Sleeps for any leftover time in a cycle. Calculated from the last time sleep, reset, or the constructor was called.
* @return True if the desired rate was met for the cycle, false otherwise.
*/
bool sleep();
/**
* @brief Sets the start time for the rate to now
*/
void reset();
/**
* @brief Get the actual run time of a cycle from start to sleep
* @return The runtime of the cycle
*/
Duration cycleTime() const;
/**
* @brief Get the expected cycle time -- one over the frequency passed in to the constructor
*/
Duration expectedCycleTime() const { return expected_cycle_time_; }
private:
Time start_;
Duration expected_cycle_time_, actual_cycle_time_;
};
/**
* @class WallRate
* @brief Class to help run loops at a desired frequency. This version always uses wall-clock time.
*/
class ROSTIME_DECL WallRate
{
public:
/**
* @brief Constructor, creates a Rate
* @param frequency The desired rate to run at in Hz
*/
WallRate(double frequency);
explicit WallRate(const Duration&);
/**
* @brief Sleeps for any leftover time in a cycle. Calculated from the last time sleep, reset, or the constructor was called.
* @return Passes through the return value from WallDuration::sleep() if it slept, false otherwise.
*/
bool sleep();
/**
* @brief Sets the start time for the rate to now
*/
void reset();
/**
* @brief Get the actual run time of a cycle from start to sleep
* @return The runtime of the cycle
*/
WallDuration cycleTime() const;
/**
* @brief Get the expected cycle time -- one over the frequency passed in to the constructor
*/
WallDuration expectedCycleTime() const { return expected_cycle_time_; }
private:
WallTime start_;
WallDuration expected_cycle_time_, actual_cycle_time_;
};
}
#endif

55
thirdparty/ros/roscpp_core/rostime/include/ros/rostime_decl.h vendored Normal file
View File

@@ -0,0 +1,55 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*********************************************************************/
/*
* Cross platform macros.
*
*/
#ifndef ROSTIME_DECL_HPP_INCLUDED
#define ROSTIME_DECL_HPP_INCLUDED
#include <ros/macros.h>
#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
#ifdef rostime_EXPORTS // we are building a shared lib/dll
#define ROSTIME_DECL ROS_HELPER_EXPORT
#else // we are using shared lib/dll
#define ROSTIME_DECL ROS_HELPER_IMPORT
#endif
#else // ros is being built around static libraries
#define ROSTIME_DECL
#endif
#endif /* ROSTIME_DECL_HPP_INCLUDED */

292
thirdparty/ros/roscpp_core/rostime/include/ros/time.h vendored Normal file
View File

@@ -0,0 +1,292 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROS_TIME_H_INCLUDED
#define ROS_TIME_H_INCLUDED
/*********************************************************************
** Pragmas
*********************************************************************/
#ifdef _MSC_VER
// Rostime has some magic interface that doesn't directly include
// its implementation, this just disables those warnings.
#pragma warning(disable: 4244)
#pragma warning(disable: 4661)
#endif
/*********************************************************************
** Headers
*********************************************************************/
#include <ros/platform.h>
#include <iostream>
#include <cmath>
#include <ros/exception.h>
#include "duration.h"
#include <boost/math/special_functions/round.hpp>
#include "rostime_decl.h"
/*********************************************************************
** Cross Platform Headers
*********************************************************************/
#if defined(_WIN32)
#include <sys/timeb.h>
#else
#include <sys/time.h>
#endif
namespace boost {
namespace posix_time {
class ptime;
class time_duration;
}
}
namespace ros
{
/*********************************************************************
** Exceptions
*********************************************************************/
/**
* @brief Thrown if the ros subsystem hasn't been initialised before use.
*/
class ROSTIME_DECL TimeNotInitializedException : public Exception
{
public:
TimeNotInitializedException()
: Exception("Cannot use ros::Time::now() before the first NodeHandle has been created or ros::start() has been called. "
"If this is a standalone app or test that just uses ros::Time and does not communicate over ROS, you may also call ros::Time::init()")
{}
};
/**
* @brief Thrown if windows high perf. timestamping is unavailable.
*
* @sa getWallTime
*/
class ROSTIME_DECL NoHighPerformanceTimersException : public Exception
{
public:
NoHighPerformanceTimersException()
: Exception("This windows platform does not "
"support the high-performance timing api.")
{}
};
/*********************************************************************
** Functions
*********************************************************************/
ROSTIME_DECL void normalizeSecNSec(uint64_t& sec, uint64_t& nsec);
ROSTIME_DECL void normalizeSecNSec(uint32_t& sec, uint32_t& nsec);
ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec);
ROSTIME_DECL void ros_walltime(uint32_t& sec, uint32_t& nsec);
ROSTIME_DECL void ros_steadytime(uint32_t& sec, uint32_t& nsec);
/*********************************************************************
** Time Classes
*********************************************************************/
/**
* \brief Base class for Time implementations. Provides storage, common functions and operator overloads.
* This should not need to be used directly.
*/
template<class T, class D>
class TimeBase
{
public:
uint32_t sec, nsec;
TimeBase() : sec(0), nsec(0) { }
TimeBase(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec)
{
normalizeSecNSec(sec, nsec);
}
explicit TimeBase(double t) { fromSec(t); }
D operator-(const T &rhs) const;
T operator+(const D &rhs) const;
T operator-(const D &rhs) const;
T& operator+=(const D &rhs);
T& operator-=(const D &rhs);
bool operator==(const T &rhs) const;
inline bool operator!=(const T &rhs) const { return !(*static_cast<const T*>(this) == rhs); }
bool operator>(const T &rhs) const;
bool operator<(const T &rhs) const;
bool operator>=(const T &rhs) const;
bool operator<=(const T &rhs) const;
double toSec() const { return static_cast<double>(sec) + 1e-9*static_cast<double>(nsec); };
T& fromSec(double t);
uint64_t toNSec() const {return static_cast<uint64_t>(sec)*1000000000ull + static_cast<uint64_t>(nsec); }
T& fromNSec(uint64_t t);
inline bool isZero() const { return sec == 0 && nsec == 0; }
inline bool is_zero() const { return isZero(); }
boost::posix_time::ptime toBoost() const;
};
/**
* \brief Time representation. May either represent wall clock time or ROS clock time.
*
* ros::TimeBase provides most of its functionality.
*/
class ROSTIME_DECL Time : public TimeBase<Time, Duration>
{
public:
Time()
: TimeBase<Time, Duration>()
{}
Time(uint32_t _sec, uint32_t _nsec)
: TimeBase<Time, Duration>(_sec, _nsec)
{}
explicit Time(double t) { fromSec(t); }
/**
* \brief Retrieve the current time. If ROS clock time is in use, this returns the time according to the
* ROS clock. Otherwise returns the current wall clock time.
*/
static Time now();
/**
* \brief Sleep until a specific time has been reached.
* @return True if the desired sleep time was met, false otherwise.
*/
static bool sleepUntil(const Time& end);
static void init();
static void shutdown();
static void setNow(const Time& new_now);
static bool useSystemTime();
static bool isSimTime();
static bool isSystemTime();
/**
* \brief Returns whether or not the current time source is valid. Simulation time is valid if it is non-zero.
*/
static bool isValid();
/**
* \brief Wait for time source to become valid
*/
static bool waitForValid();
/**
* \brief Wait for time source to become valid, with timeout
*/
static bool waitForValid(const ros::WallDuration& timeout);
static Time fromBoost(const boost::posix_time::ptime& t);
static Time fromBoost(const boost::posix_time::time_duration& d);
};
extern ROSTIME_DECL const Time TIME_MAX;
extern ROSTIME_DECL const Time TIME_MIN;
/**
* \brief Time representation. Always wall-clock time.
*
* ros::TimeBase provides most of its functionality.
*/
class ROSTIME_DECL WallTime : public TimeBase<WallTime, WallDuration>
{
public:
WallTime()
: TimeBase<WallTime, WallDuration>()
{}
WallTime(uint32_t _sec, uint32_t _nsec)
: TimeBase<WallTime, WallDuration>(_sec, _nsec)
{}
explicit WallTime(double t) { fromSec(t); }
/**
* \brief Returns the current wall clock time.
*/
static WallTime now();
/**
* \brief Sleep until a specific time has been reached.
* @return True if the desired sleep time was met, false otherwise.
*/
static bool sleepUntil(const WallTime& end);
static bool isSystemTime() { return true; }
};
/**
* \brief Time representation. Always steady-clock time.
*
* Not affected by ROS time.
*
* ros::TimeBase provides most of its functionality.
*/
class ROSTIME_DECL SteadyTime : public TimeBase<SteadyTime, WallDuration>
{
public:
SteadyTime()
: TimeBase<SteadyTime, WallDuration>()
{}
SteadyTime(uint32_t _sec, uint32_t _nsec)
: TimeBase<SteadyTime, WallDuration>(_sec, _nsec)
{}
explicit SteadyTime(double t) { fromSec(t); }
/**
* \brief Returns the current steady (monotonic) clock time.
*/
static SteadyTime now();
/**
* \brief Sleep until a specific time has been reached.
* @return True if the desired sleep time was met, false otherwise.
*/
static bool sleepUntil(const SteadyTime& end);
static bool isSystemTime() { return true; }
};
ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const Time &rhs);
ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const WallTime &rhs);
ROSTIME_DECL std::ostream &operator <<(std::ostream &os, const SteadyTime &rhs);
}
#endif // ROS_TIME_H

24
thirdparty/ros/roscpp_core/rostime/package.xml vendored Normal file
View File

@@ -0,0 +1,24 @@
<package>
<name>rostime</name>
<version>0.7.2</version>
<description>
Time and Duration implementations for C++ libraries, including roscpp.
</description>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/rostime</url>
<author>Josh Faust</author>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libboost-date-time-dev</build_depend>
<build_depend>libboost-system-dev</build_depend>
<build_depend>libboost-thread-dev</build_depend>
<build_depend>cpp_common</build_depend>
<run_depend>libboost-date-time-dev</run_depend>
<run_depend>libboost-system-dev</run_depend>
<run_depend>libboost-thread-dev</run_depend>
<run_depend>cpp_common</run_depend>
</package>

77
thirdparty/ros/roscpp_core/rostime/src/duration.cpp vendored Normal file
View File

@@ -0,0 +1,77 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <ros/duration.h>
#include <ros/impl/duration.h>
namespace ros {
Duration::Duration(const Rate& rate)
: DurationBase<Duration>(rate.expectedCycleTime().sec, rate.expectedCycleTime().nsec)
{ }
WallDuration::WallDuration(const Rate& rate)
: DurationBase<WallDuration>(rate.expectedCycleTime().sec, rate.expectedCycleTime().nsec)
{ }
void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec)
{
int64_t nsec_part = nsec % 1000000000L;
int64_t sec_part = sec + nsec / 1000000000L;
if (nsec_part < 0)
{
nsec_part += 1000000000L;
--sec_part;
}
if (sec_part < std::numeric_limits<int32_t>::min() || sec_part > std::numeric_limits<int32_t>::max())
throw std::runtime_error("Duration is out of dual 32-bit range");
sec = sec_part;
nsec = nsec_part;
}
void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec)
{
int64_t sec64 = sec;
int64_t nsec64 = nsec;
normalizeSecNSecSigned(sec64, nsec64);
sec = (int32_t)sec64;
nsec = (int32_t)nsec64;
}
template class DurationBase<Duration>;
template class DurationBase<WallDuration>;
}

159
thirdparty/ros/roscpp_core/rostime/src/rate.cpp vendored Normal file
View File

@@ -0,0 +1,159 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <ros/rate.h>
namespace ros
{
Rate::Rate(double frequency)
: start_(Time::now())
, expected_cycle_time_(1.0 / frequency)
, actual_cycle_time_(0.0)
{ }
Rate::Rate(const Duration& d)
: start_(Time::now())
, expected_cycle_time_(d.sec, d.nsec)
, actual_cycle_time_(0.0)
{ }
bool Rate::sleep()
{
Time expected_end = start_ + expected_cycle_time_;
Time actual_end = Time::now();
// detect backward jumps in time
if (actual_end < start_)
{
expected_end = actual_end + expected_cycle_time_;
}
//calculate the time we'll sleep for
Duration sleep_time = expected_end - actual_end;
//set the actual amount of time the loop took in case the user wants to know
actual_cycle_time_ = actual_end - start_;
//make sure to reset our start time
start_ = expected_end;
//if we've taken too much time we won't sleep
if(sleep_time <= Duration(0.0))
{
// if we've jumped forward in time, or the loop has taken more than a full extra
// cycle, reset our cycle
if (actual_end > expected_end + expected_cycle_time_)
{
start_ = actual_end;
}
// return false to show that the desired rate was not met
return false;
}
return sleep_time.sleep();
}
void Rate::reset()
{
start_ = Time::now();
}
Duration Rate::cycleTime() const
{
return actual_cycle_time_;
}
WallRate::WallRate(double frequency)
: start_(WallTime::now())
, expected_cycle_time_(1.0 / frequency)
, actual_cycle_time_(0.0)
{}
WallRate::WallRate(const Duration& d)
: start_(WallTime::now())
, expected_cycle_time_(d.sec, d.nsec)
, actual_cycle_time_(0.0)
{}
bool WallRate::sleep()
{
WallTime expected_end = start_ + expected_cycle_time_;
WallTime actual_end = WallTime::now();
// detect backward jumps in time
if (actual_end < start_)
{
expected_end = actual_end + expected_cycle_time_;
}
//calculate the time we'll sleep for
WallDuration sleep_time = expected_end - actual_end;
//set the actual amount of time the loop took in case the user wants to know
actual_cycle_time_ = actual_end - start_;
//make sure to reset our start time
start_ = expected_end;
//if we've taken too much time we won't sleep
if(sleep_time <= WallDuration(0.0))
{
// if we've jumped forward in time, or the loop has taken more than a full extra
// cycle, reset our cycle
if (actual_end > expected_end + expected_cycle_time_)
{
start_ = actual_end;
}
return false;
}
return sleep_time.sleep();
}
void WallRate::reset()
{
start_ = WallTime::now();
}
WallDuration WallRate::cycleTime() const
{
return actual_cycle_time_;
}
}

516
thirdparty/ros/roscpp_core/rostime/src/time.cpp vendored Normal file
View File

@@ -0,0 +1,516 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifdef _MSC_VER
#ifndef NOMINMAX
#define NOMINMAX
#endif
#endif
#include "ros/time.h"
#include "ros/impl/time.h"
#include <cmath>
#include <ctime>
#include <iomanip>
#include <limits>
#include <stdexcept>
// time related includes for macOS
#if defined(__APPLE__)
#include <mach/clock.h>
#include <mach/mach.h>
#endif // defined(__APPLE__)
#ifdef _WINDOWS
#include <chrono>
#include <thread>
#include <windows.h>
#endif
#include <boost/thread/mutex.hpp>
#include <boost/io/ios_state.hpp>
#include <boost/date_time/posix_time/ptime.hpp>
/*********************************************************************
** Preprocessor
*********************************************************************/
// Could probably do some better and more elaborate checking
// and definition here.
#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L)
/*********************************************************************
** Namespaces
*********************************************************************/
namespace ros
{
/*********************************************************************
** Variables
*********************************************************************/
const Duration DURATION_MAX(std::numeric_limits<int32_t>::max(), 999999999);
const Duration DURATION_MIN(std::numeric_limits<int32_t>::min(), 0);
const Time TIME_MAX(std::numeric_limits<uint32_t>::max(), 999999999);
const Time TIME_MIN(0, 1);
// This is declared here because it's set from the Time class but read from
// the Duration class, and need not be exported to users of either.
static bool g_stopped(false);
// I assume that this is declared here, instead of time.h, to keep users
// of time.h from including boost/thread/mutex.hpp
static boost::mutex g_sim_time_mutex;
static bool g_initialized(false);
static bool g_use_sim_time(true);
static Time g_sim_time(0, 0);
/*********************************************************************
** Cross Platform Functions
*********************************************************************/
void ros_walltime(uint32_t& sec, uint32_t& nsec)
{
#if !defined(_WIN32)
#if HAS_CLOCK_GETTIME
timespec start;
clock_gettime(CLOCK_REALTIME, &start);
if (start.tv_sec < 0 || start.tv_sec > std::numeric_limits<uint32_t>::max())
throw std::runtime_error("Timespec is out of dual 32-bit range");
sec = start.tv_sec;
nsec = start.tv_nsec;
#else
struct timeval timeofday;
gettimeofday(&timeofday,NULL);
if (timeofday.tv_sec < 0 || timeofday.tv_sec > std::numeric_limits<uint32_t>::max())
throw std::runtime_error("Timeofday is out of dual signed 32-bit range");
sec = timeofday.tv_sec;
nsec = timeofday.tv_usec * 1000;
#endif
#else
uint64_t now_s = 0;
uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
normalizeSecNSec(now_s, now_ns);
sec = (uint32_t)now_s;
nsec = (uint32_t)now_ns;
#endif
}
void ros_steadytime(uint32_t& sec, uint32_t& nsec)
{
#if !defined(_WIN32)
timespec start;
#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);
start.tv_sec = mts.tv_sec;
start.tv_nsec = mts.tv_nsec;
#else // defined(__APPLE__)
// Otherwise use clock_gettime.
clock_gettime(CLOCK_MONOTONIC, &start);
#endif // defined(__APPLE__)
sec = start.tv_sec;
nsec = start.tv_nsec;
#else
uint64_t now_s = 0;
uint64_t now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now().time_since_epoch()).count();
normalizeSecNSec(now_s, now_ns);
sec = (uint32_t)now_s;
nsec = (uint32_t)now_ns;
#endif
}
/*
* These have only internal linkage to this translation unit.
* (i.e. not exposed to users of the time classes)
*/
/**
* @brief Simple representation of the rt library nanosleep function.
*/
int ros_nanosleep(const uint32_t &sec, const uint32_t &nsec)
{
#if defined(_WIN32)
std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast<int64_t>(sec * 1e9 + nsec)));
return 0;
#else
timespec req = { sec, nsec };
return nanosleep(&req, NULL);
#endif
}
/**
* @brief Go to the wall!
*
* @todo Fully implement the win32 parts, currently just like a regular sleep.
*/
bool ros_wallsleep(uint32_t sec, uint32_t nsec)
{
#if defined(_WIN32)
ros_nanosleep(sec,nsec);
#else
timespec req = { sec, nsec };
timespec rem = {0, 0};
while (nanosleep(&req, &rem) && !g_stopped)
{
req = rem;
}
#endif
return !g_stopped;
}
/*********************************************************************
** Class Methods
*********************************************************************/
bool Time::useSystemTime()
{
return !g_use_sim_time;
}
bool Time::isSimTime()
{
return g_use_sim_time;
}
bool Time::isSystemTime()
{
return !isSimTime();
}
Time Time::now()
{
if (!g_initialized)
{
throw TimeNotInitializedException();
}
if (g_use_sim_time)
{
boost::mutex::scoped_lock lock(g_sim_time_mutex);
Time t = g_sim_time;
return t;
}
Time t;
ros_walltime(t.sec, t.nsec);
return t;
}
void Time::setNow(const Time& new_now)
{
boost::mutex::scoped_lock lock(g_sim_time_mutex);
g_sim_time = new_now;
g_use_sim_time = true;
}
void Time::init()
{
g_stopped = false;
g_use_sim_time = false;
g_initialized = true;
}
void Time::shutdown()
{
g_stopped = true;
}
bool Time::isValid()
{
return (!g_use_sim_time) || !g_sim_time.isZero();
}
bool Time::waitForValid()
{
return waitForValid(ros::WallDuration());
}
bool Time::waitForValid(const ros::WallDuration& timeout)
{
ros::WallTime start = ros::WallTime::now();
while (!isValid() && !g_stopped)
{
ros::WallDuration(0.01).sleep();
if (timeout > ros::WallDuration(0, 0) && (ros::WallTime::now() - start > timeout))
{
return false;
}
}
if (g_stopped)
{
return false;
}
return true;
}
Time Time::fromBoost(const boost::posix_time::ptime& t)
{
boost::posix_time::time_duration diff = t - boost::posix_time::from_time_t(0);
return Time::fromBoost(diff);
}
Time Time::fromBoost(const boost::posix_time::time_duration& d)
{
Time t;
int64_t sec64 = d.total_seconds();
if (sec64 < 0 || sec64 > std::numeric_limits<uint32_t>::max())
throw std::runtime_error("time_duration is out of dual 32-bit range");
t.sec = (uint32_t)sec64;
#if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
t.nsec = d.fractional_seconds();
#else
t.nsec = d.fractional_seconds()*1000;
#endif
return t;
}
std::ostream& operator<<(std::ostream& os, const Time &rhs)
{
boost::io::ios_all_saver s(os);
os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
return os;
}
std::ostream& operator<<(std::ostream& os, const Duration& rhs)
{
boost::io::ios_all_saver s(os);
if (rhs.sec >= 0 || rhs.nsec == 0)
{
os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
}
else
{
os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
}
return os;
}
bool Time::sleepUntil(const Time& end)
{
if (Time::useSystemTime())
{
Duration d(end - Time::now());
if (d > Duration(0))
{
return d.sleep();
}
return true;
}
else
{
Time start = Time::now();
while (!g_stopped && (Time::now() < end))
{
ros_nanosleep(0,1000000);
if (Time::now() < start)
{
return false;
}
}
return true;
}
}
bool WallTime::sleepUntil(const WallTime& end)
{
WallDuration d(end - WallTime::now());
if (d > WallDuration(0))
{
return d.sleep();
}
return true;
}
bool SteadyTime::sleepUntil(const SteadyTime& end)
{
WallDuration d(end - SteadyTime::now());
if (d > WallDuration(0))
{
return d.sleep();
}
return true;
}
bool Duration::sleep() const
{
if (Time::useSystemTime())
{
return ros_wallsleep(sec, nsec);
}
else
{
Time start = Time::now();
Time end = start + *this;
if (start.isZero())
{
end = TIME_MAX;
}
bool rc = false;
while (!g_stopped && (Time::now() < end))
{
ros_wallsleep(0, 1000000);
rc = true;
// If we started at time 0 wait for the first actual time to arrive before starting the timer on
// our sleep
if (start.isZero())
{
start = Time::now();
end = start + *this;
}
// If time jumped backwards from when we started sleeping, return immediately
if (Time::now() < start)
{
return false;
}
}
return rc && !g_stopped;
}
}
std::ostream &operator<<(std::ostream& os, const WallTime &rhs)
{
boost::io::ios_all_saver s(os);
os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
return os;
}
std::ostream &operator<<(std::ostream& os, const SteadyTime &rhs)
{
boost::io::ios_all_saver s(os);
os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
return os;
}
WallTime WallTime::now()
{
WallTime t;
ros_walltime(t.sec, t.nsec);
return t;
}
SteadyTime SteadyTime::now()
{
SteadyTime t;
ros_steadytime(t.sec, t.nsec);
return t;
}
std::ostream &operator<<(std::ostream& os, const WallDuration& rhs)
{
boost::io::ios_all_saver s(os);
if (rhs.sec >= 0 || rhs.nsec == 0)
{
os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec;
}
else
{
os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec);
}
return os;
}
bool WallDuration::sleep() const
{
return ros_wallsleep(sec, nsec);
}
void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)
{
uint64_t nsec_part = nsec % 1000000000UL;
uint64_t sec_part = nsec / 1000000000UL;
if (sec + sec_part > std::numeric_limits<uint32_t>::max())
throw std::runtime_error("Time is out of dual 32-bit range");
sec += sec_part;
nsec = nsec_part;
}
void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)
{
uint64_t sec64 = sec;
uint64_t nsec64 = nsec;
normalizeSecNSec(sec64, nsec64);
sec = (uint32_t)sec64;
nsec = (uint32_t)nsec64;
}
void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)
{
int64_t nsec_part = nsec % 1000000000L;
int64_t sec_part = sec + nsec / 1000000000L;
if (nsec_part < 0)
{
nsec_part += 1000000000L;
--sec_part;
}
if (sec_part < 0 || sec_part > std::numeric_limits<uint32_t>::max())
throw std::runtime_error("Time is out of dual 32-bit range");
sec = sec_part;
nsec = nsec_part;
}
template class TimeBase<Time, Duration>;
template class TimeBase<WallTime, WallDuration>;
template class TimeBase<SteadyTime, WallDuration>;
}

167
thirdparty/ros/roscpp_core/rostime/test/duration.cpp vendored Normal file
View File

@@ -0,0 +1,167 @@
/*
* Copyright (c) 2016, Open Source Robotics Foundation, Inc..
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <ros/duration.h>
#include <ros/time.h>
using namespace ros;
TEST(Duration, sleepWithSimTime)
{
ros::Time::init();
Time start = Time::now();
start -= Duration(2.0);
Time::setNow(start);
Time::shutdown();
Duration d(1.0);
bool rc = d.sleep();
ASSERT_FALSE(rc);
}
TEST(Duration, castFromDoubleExceptions)
{
ros::Time::init();
Duration d1, d2, d3, d4;
// Valid values to cast, must not throw exceptions
EXPECT_NO_THROW(d1.fromSec(-2147483648.0));
EXPECT_NO_THROW(d2.fromSec(-2147483647.999999));
EXPECT_NO_THROW(d3.fromSec(2147483647.0));
EXPECT_NO_THROW(d4.fromSec(2147483647.999999));
// The next casts all incorrect.
EXPECT_THROW(d1.fromSec(2147483648.0), std::runtime_error);
EXPECT_THROW(d2.fromSec(6442450943.0), std::runtime_error); // It's 2^31 - 1 + 2^32, and it could pass the test.
EXPECT_THROW(d3.fromSec(-2147483648.001), std::runtime_error);
EXPECT_THROW(d4.fromSec(-6442450943.0), std::runtime_error);
}
TEST(Duration, castFromInt64Exceptions)
{
ros::Time::init();
Duration d1, d2, d3, d4;
// Valid values to cast, must not throw exceptions
EXPECT_NO_THROW(d1.fromNSec(-2147483648000000000));
EXPECT_NO_THROW(d2.fromNSec(-2147483647999999999));
EXPECT_NO_THROW(d3.fromNSec(2147483647000000000));
EXPECT_NO_THROW(d4.fromNSec(2147483647999999999));
// The next casts all incorrect.
EXPECT_THROW(d1.fromSec(2147483648000000000), std::runtime_error);
EXPECT_THROW(d2.fromSec(4294967296000000000), std::runtime_error);
EXPECT_THROW(d3.fromSec(-2147483648000000001), std::runtime_error);
EXPECT_THROW(d4.fromSec(-6442450943000000000), std::runtime_error);
}
TEST(Duration, arithmeticExceptions)
{
ros::Time::init();
Duration d1(2147483647, 0);
Duration d2(2147483647, 999999999);
EXPECT_THROW(d1 + d2, std::runtime_error);
Duration d3(-2147483648, 0);
Duration d4(2147483647, 0);
EXPECT_THROW(d3 - d4, std::runtime_error);
EXPECT_THROW(d4 - d3, std::runtime_error);
Duration d5(-2147483647, 1);
Duration d6(-2, 999999999);
Duration d7;
EXPECT_NO_THROW(d7 = d5 + d6);
EXPECT_EQ(-2147483648000000000, d7.toNSec());
}
TEST(Duration, negativeSignExceptions)
{
ros::Time::init();
Duration d1(2147483647, 0);
Duration d2(2147483647, 999999999);
Duration d3;
EXPECT_NO_THROW(d3 = -d1);
EXPECT_EQ(-2147483647000000000, d3.toNSec());
EXPECT_NO_THROW(d3 = -d2);
EXPECT_EQ(-2147483647999999999, d3.toNSec());
Duration d4(-2147483647, 0);
Duration d5(-2147483648, 999999999);
Duration d6(-2147483648, 2);
Duration d7;
EXPECT_NO_THROW(d7 = -d4);
EXPECT_EQ(2147483647000000000, d7.toNSec());
EXPECT_NO_THROW(d7 = -d5);
EXPECT_EQ(2147483647000000001, d7.toNSec());
EXPECT_NO_THROW(d7 = -d6);
EXPECT_EQ(2147483647999999998, d7.toNSec());
}
TEST(Duration, rounding)
{
ros::Time::init();
Duration d1(49.0000000004);
EXPECT_EQ(49, d1.sec);
EXPECT_EQ(0, d1.nsec);
Duration d2(-49.0000000004);
EXPECT_EQ(-49, d2.sec);
EXPECT_EQ(0, d2.nsec);
Duration d3(49.0000000006);
EXPECT_EQ(49, d3.sec);
EXPECT_EQ(1, d3.nsec);
Duration d4(-49.0000000006);
EXPECT_EQ(-50, d4.sec);
EXPECT_EQ(999999999, d4.nsec);
Duration d5(49.9999999994);
EXPECT_EQ(49, d5.sec);
EXPECT_EQ(999999999, d5.nsec);
Duration d6(-49.9999999994);
EXPECT_EQ(-50, d6.sec);
EXPECT_EQ(1, d6.nsec);
Duration d7(49.9999999996);
EXPECT_EQ(50, d7.sec);
EXPECT_EQ(0, d7.nsec);
Duration d8(-49.9999999996);
EXPECT_EQ(-50, d8.sec);
EXPECT_EQ(0, d8.nsec);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

611
thirdparty/ros/roscpp_core/rostime/test/time.cpp vendored Normal file
View File

@@ -0,0 +1,611 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <vector>
#include <gtest/gtest.h>
#include <ros/rate.h>
#include <ros/time.h>
#if !defined(_WIN32)
#include <sys/time.h>
#endif
#include <boost/date_time/posix_time/ptime.hpp>
using namespace ros;
/// \todo All the tests in here that use randomized values are not unit tests, replace them
double epsilon = 1e-9;
void seed_rand()
{
//Seed random number generator with current microseond count
#if !defined(_WIN32)
timeval temp_time_struct;
gettimeofday(&temp_time_struct,NULL);
srand(temp_time_struct.tv_usec);
#else
srand(time(nullptr));
#endif
};
void generate_rand_times(uint32_t range, uint64_t runs, std::vector<ros::Time>& values1, std::vector<ros::Time>& values2)
{
seed_rand();
values1.clear();
values2.clear();
values1.reserve(runs);
values2.reserve(runs);
for ( uint32_t i = 0; i < runs ; i++ )
{
values1.push_back(ros::Time( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
values2.push_back(ros::Time( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
}
}
void generate_rand_durations(uint32_t range, uint64_t runs, std::vector<ros::Duration>& values1, std::vector<ros::Duration>& values2)
{
seed_rand();
values1.clear();
values2.clear();
values1.reserve(runs * 4);
values2.reserve(runs * 4);
for ( uint32_t i = 0; i < runs ; i++ )
{
// positive durations
values1.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
values2.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
EXPECT_GE(values1.back(), ros::Duration(0,0));
EXPECT_GE(values2.back(), ros::Duration(0,0));
// negative durations
values1.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
values2.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
EXPECT_LE(values1.back(), ros::Duration(0,0));
EXPECT_LE(values2.back(), ros::Duration(0,0));
// positive and negative durations
values1.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
values2.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
EXPECT_GE(values1.back(), ros::Duration(0,0));
EXPECT_LE(values2.back(), ros::Duration(0,0));
// negative and positive durations
values1.push_back(ros::Duration( -(rand() * range / RAND_MAX), -(rand() * 1000000000ULL/RAND_MAX)));
values2.push_back(ros::Duration( (rand() * range / RAND_MAX), (rand() * 1000000000ULL/RAND_MAX)));
EXPECT_LE(values1.back(), ros::Duration(0,0));
EXPECT_GE(values2.back(), ros::Duration(0,0));
}
}
TEST(Time, size)
{
ASSERT_EQ(sizeof(Time), 8);
ASSERT_EQ(sizeof(Duration), 8);
}
TEST(Time, Comparitors)
{
std::vector<ros::Time> v1;
std::vector<ros::Time> v2;
generate_rand_times(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
if (v1[i].sec * 1000000000ULL + v1[i].nsec < v2[i].sec * 1000000000ULL + v2[i].nsec)
{
EXPECT_LT(v1[i], v2[i]);
// printf("%f %d ", v1[i].toSec(), v1[i].sec * 1000000000ULL + v1[i].nsec);
//printf("vs %f %d\n", v2[i].toSec(), v2[i].sec * 1000000000ULL + v2[i].nsec);
EXPECT_LE(v1[i], v2[i]);
EXPECT_NE(v1[i], v2[i]);
}
else if (v1[i].sec * 1000000000ULL + v1[i].nsec > v2[i].sec * 1000000000ULL + v2[i].nsec)
{
EXPECT_GT(v1[i], v2[i]);
EXPECT_GE(v1[i], v2[i]);
EXPECT_NE(v1[i], v2[i]);
}
else
{
EXPECT_EQ(v1[i], v2[i]);
EXPECT_LE(v1[i], v2[i]);
EXPECT_GE(v1[i], v2[i]);
}
}
}
TEST(Time, ToFromDouble)
{
std::vector<ros::Time> v1;
std::vector<ros::Time> v2;
generate_rand_times(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
EXPECT_EQ(v1[i].toSec(), v1[i].fromSec(v1[i].toSec()).toSec());
}
}
TEST(Time, RoundingError)
{
double someInt = 1031.0; // some integer
double t = std::nextafter(someInt, 0); // someint - epsilon
// t should be 1031.000000
ros::Time exampleTime;
exampleTime.fromSec(t);
// if rounded incorrectly, sec may be 1030
// and nsec may be 1e9.
EXPECT_EQ(exampleTime.sec, 1031);
EXPECT_EQ(exampleTime.nsec, 0);
}
TEST(Time, OperatorPlus)
{
Time t(100, 0);
Duration d(100, 0);
Time r = t + d;
EXPECT_EQ(r.sec, 200UL);
EXPECT_EQ(r.nsec, 0UL);
t = Time(0, 100000UL);
d = Duration(0, 100UL);
r = t + d;
EXPECT_EQ(r.sec, 0UL);
EXPECT_EQ(r.nsec, 100100UL);
t = Time(0, 0);
d = Duration(10, 2000003000UL);
r = t + d;
EXPECT_EQ(r.sec, 12UL);
EXPECT_EQ(r.nsec, 3000UL);
}
TEST(Time, OperatorMinus)
{
Time t(100, 0);
Duration d(100, 0);
Time r = t - d;
EXPECT_EQ(r.sec, 0UL);
EXPECT_EQ(r.nsec, 0UL);
t = Time(0, 100000UL);
d = Duration(0, 100UL);
r = t - d;
EXPECT_EQ(r.sec, 0UL);
EXPECT_EQ(r.nsec, 99900UL);
t = Time(30, 0);
d = Duration(10, 2000003000UL);
r = t - d;
EXPECT_EQ(r.sec, 17UL);
EXPECT_EQ(r.nsec, 999997000ULL);
}
TEST(Time, OperatorPlusEquals)
{
Time t(100, 0);
Duration d(100, 0);
t += d;
EXPECT_EQ(t.sec, 200UL);
EXPECT_EQ(t.nsec, 0UL);
t = Time(0, 100000UL);
d = Duration(0, 100UL);
t += d;
EXPECT_EQ(t.sec, 0UL);
EXPECT_EQ(t.nsec, 100100UL);
t = Time(0, 0);
d = Duration(10, 2000003000UL);
t += d;
EXPECT_EQ(t.sec, 12UL);
EXPECT_EQ(t.nsec, 3000UL);
}
TEST(Time, OperatorMinusEquals)
{
Time t(100, 0);
Duration d(100, 0);
t -= d;
EXPECT_EQ(t.sec, 0UL);
EXPECT_EQ(t.nsec, 0UL);
t = Time(0, 100000UL);
d = Duration(0, 100UL);
t -= d;
EXPECT_EQ(t.sec, 0UL);
EXPECT_EQ(t.nsec, 99900UL);
t = Time(30, 0);
d = Duration(10, 2000003000UL);
t -= d;
EXPECT_EQ(t.sec, 17UL);
EXPECT_EQ(t.nsec, 999997000ULL);
}
TEST(Time, SecNSecConstructor)
{
Time t(100, 2000003000UL);
EXPECT_EQ(t.sec, 102UL);
EXPECT_EQ(t.nsec, 3000UL);
}
TEST(Time, DontMungeStreamState)
{
std::ostringstream oss;
Time t(100, 2000003000UL);
oss << std::setfill('N');
oss << std::setw(13);
oss << t;
EXPECT_EQ(oss.width(), 13);
EXPECT_EQ(oss.fill(), 'N');
}
TEST(Time, ToFromBoost)
{
std::vector<ros::Time> v1;
std::vector<ros::Time> v2;
generate_rand_times(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
Time t = v1[i];
// dont assume that nanosecond are available
t.nsec = uint32_t(t.nsec / 1000.0) * 1000;
boost::posix_time::ptime b = t.toBoost();
Time tt = Time::fromBoost(b);
EXPECT_EQ(t, tt);
}
}
TEST(Time, CastFromDoubleExceptions)
{
ros::Time::init();
Time t1, t2, t3;
// Valid values to cast, must not throw exceptions
EXPECT_NO_THROW(t1.fromSec(4294967295.0));
EXPECT_NO_THROW(t2.fromSec(4294967295.999));
EXPECT_NO_THROW(t3.fromSec(0.0000001));
// The next casts all incorrect.
EXPECT_THROW(t1.fromSec(4294967296.0), std::runtime_error);
EXPECT_THROW(t2.fromSec(-0.0001), std::runtime_error);
EXPECT_THROW(t3.fromSec(-4294967296.0), std::runtime_error);
}
TEST(Time, OperatorMinusExceptions)
{
ros::Time::init();
Time t1(2147483648, 0);
Time t2(2147483647, 999999999);
Time t3(2147483647, 999999998);
Time t4(4294967295, 999999999);
Time t5(4294967295, 999999998);
Time t6(0, 1);
Duration d1(2147483647, 999999999);
Duration d2(-2147483648, 0);
Duration d3(-2147483648, 1);
Duration d4(0, 1);
EXPECT_NO_THROW(t1 - t2);
EXPECT_NO_THROW(t3 - t2);
EXPECT_NO_THROW(t4 - t5);
EXPECT_NO_THROW(t1 - d1);
EXPECT_NO_THROW(t5 - d1);
EXPECT_THROW(t4 - t6, std::runtime_error);
EXPECT_THROW(t4 - t3, std::runtime_error);
EXPECT_THROW(t1 - d2, std::runtime_error);
EXPECT_THROW(t2 - d2, std::runtime_error);
EXPECT_THROW(t4 - d3, std::runtime_error);
}
TEST(Time, OperatorPlusExceptions)
{
ros::Time::init();
Time t1(2147483648, 0);
Time t2(2147483647, 999999999);
Time t4(4294967295, 999999999);
Time t5(4294967295, 999999998);
Duration d1(2147483647, 999999999);
Duration d2(-2147483648, 1);
Duration d3(0, 2);
Duration d4(0, 1);
EXPECT_NO_THROW(t2 + d2);
EXPECT_NO_THROW(t1 + d1);
EXPECT_THROW(t4 + d4, std::runtime_error);
EXPECT_THROW(t4 + d1, std::runtime_error);
EXPECT_THROW(t5 + d3, std::runtime_error);
}
/************************************* Duration Tests *****************/
TEST(Duration, Comparitors)
{
std::vector<ros::Duration> v1;
std::vector<ros::Duration> v2;
generate_rand_durations(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
if (v1[i].sec * 1000000000LL + v1[i].nsec < v2[i].sec * 1000000000LL + v2[i].nsec)
{
EXPECT_LT(v1[i], v2[i]);
// printf("%f %lld ", v1[i].toSec(), v1[i].sec * 1000000000LL + v1[i].nsec);
// printf("vs %f %lld\n", v2[i].toSec(), v2[i].sec * 1000000000LL + v2[i].nsec);
EXPECT_LE(v1[i], v2[i]);
EXPECT_NE(v1[i], v2[i]);
}
else if (v1[i].sec * 1000000000LL + v1[i].nsec > v2[i].sec * 1000000000LL + v2[i].nsec)
{
EXPECT_GT(v1[i], v2[i]);
// printf("%f %lld ", v1[i].toSec(), v1[i].sec * 1000000000LL + v1[i].nsec);
// printf("vs %f %lld\n", v2[i].toSec(), v2[i].sec * 1000000000LL + v2[i].nsec);
EXPECT_GE(v1[i], v2[i]);
EXPECT_NE(v1[i], v2[i]);
}
else
{
EXPECT_EQ(v1[i], v2[i]);
EXPECT_LE(v1[i], v2[i]);
EXPECT_GE(v1[i], v2[i]);
}
}
}
TEST(Duration, ToFromSec)
{
std::vector<ros::Duration> v1;
std::vector<ros::Duration> v2;
generate_rand_durations(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
EXPECT_EQ(v1[i].toSec(), v1[i].fromSec(v1[i].toSec()).toSec());
EXPECT_GE(ros::Duration(v1[i].toSec()).nsec, 0);
}
EXPECT_EQ(ros::Duration(-0.5), ros::Duration(-1LL, 500000000LL));
EXPECT_EQ(ros::Duration(-0.5), ros::Duration(0, -500000000LL));
}
TEST(Duration, FromNSec)
{
ros::Duration t;
t.fromNSec(-500000000LL);
EXPECT_EQ(ros::Duration(-0.5), t);
t.fromNSec(-1500000000LL);
EXPECT_EQ(ros::Duration(-1.5), t);
t.fromNSec(500000000LL);
EXPECT_EQ(ros::Duration(0.5), t);
t.fromNSec(1500000000LL);
EXPECT_EQ(ros::Duration(1.5), t);
}
TEST(Duration, OperatorPlus)
{
std::vector<ros::Duration> v1;
std::vector<ros::Duration> v2;
generate_rand_durations(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
EXPECT_NEAR(v1[i].toSec() + v2[i].toSec(), (v1[i] + v2[i]).toSec(), epsilon);
ros::Duration temp = v1[i];
EXPECT_NEAR(v1[i].toSec() + v2[i].toSec(), (temp += v2[i]).toSec(), epsilon);
}
}
TEST(Duration, OperatorMinus)
{
std::vector<ros::Duration> v1;
std::vector<ros::Duration> v2;
generate_rand_durations(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
EXPECT_NEAR(v1[i].toSec() - v2[i].toSec(), (v1[i] - v2[i]).toSec(), epsilon);
ros::Duration temp = v1[i];
EXPECT_NEAR(v1[i].toSec() - v2[i].toSec(), (temp -= v2[i]).toSec(), epsilon);
EXPECT_NEAR(- v2[i].toSec(), (-v2[i]).toSec(), epsilon);
}
ros::Time t1(1.1);
ros::Time t2(1.3);
ros::Duration time_diff = t1 - t2; //=-0.2
EXPECT_NEAR(time_diff.toSec(), -0.2, epsilon);
EXPECT_LE(time_diff, ros::Duration(-0.19));
EXPECT_GE(time_diff, ros::Duration(-0.21));
}
TEST(Duration, OperatorTimes)
{
std::vector<ros::Duration> v1;
std::vector<ros::Duration> v2;
generate_rand_durations(100, 1000, v1,v2);
for (uint32_t i = 0; i < v1.size(); i++)
{
EXPECT_NEAR(v1[i].toSec() * v2[i].toSec(), (v1[i] * v2[i].toSec()).toSec(), epsilon);
ros::Duration temp = v1[i];
EXPECT_NEAR(v1[i].toSec() * v2[i].toSec(), (temp *= v2[i].toSec()).toSec(), epsilon);
}
}
TEST(Duration, OperatorPlusEquals)
{
Duration t(100, 0);
Duration d(100, 0);
t += d;
EXPECT_EQ(t.sec, 200L);
EXPECT_EQ(t.nsec, 0L);
t = Duration(0, 100000L);
d = Duration(0, 100L);
t += d;
EXPECT_EQ(t.sec, 0L);
EXPECT_EQ(t.nsec, 100100L);
t = Duration(0, 0);
d = Duration(10, 2000003000L);
t += d;
EXPECT_EQ(t.sec, 12L);
EXPECT_EQ(t.nsec, 3000L);
}
TEST(Duration, OperatorMinusEquals)
{
Duration t(100, 0);
Duration d(100, 0);
t -= d;
EXPECT_EQ(t.sec, 0L);
EXPECT_EQ(t.nsec, 0L);
t = Duration(0, 100000L);
d = Duration(0, 100L);
t -= d;
EXPECT_EQ(t.sec, 0L);
EXPECT_EQ(t.nsec, 99900L);
t = Duration(30, 0);
d = Duration(10, 2000003000L);
t -= d;
EXPECT_EQ(t.sec, 17L);
EXPECT_EQ(t.nsec, 999997000L);
}
void alarmHandler(int sig)
{
}
TEST(Duration, sleepWithSignal)
{
#if !defined(_WIN32)
signal(SIGALRM, alarmHandler);
alarm(1);
#endif
Time start = Time::now();
Duration d(2.0);
bool rc = d.sleep();
Time end = Time::now();
ASSERT_GT(end - start, d);
ASSERT_TRUE(rc);
}
TEST(Rate, constructFromDuration){
Duration d(4, 0);
Rate r(d);
EXPECT_EQ(r.expectedCycleTime(), d);
}
TEST(Rate, sleep_return_value_true){
Rate r(Duration(0.2));
Duration(r.expectedCycleTime() * 0.5).sleep();
EXPECT_TRUE(r.sleep());
}
TEST(Rate, sleep_return_value_false){
Rate r(Duration(0.2));
Duration(r.expectedCycleTime() * 2).sleep();
EXPECT_FALSE(r.sleep()); // requested rate cannot be achieved
}
TEST(WallRate, constructFromDuration){
Duration d(4, 0);
WallRate r(d);
WallDuration wd(4, 0);
EXPECT_EQ(r.expectedCycleTime(), wd);
}
///////////////////////////////////////////////////////////////////////////////////
// WallTime/WallDuration
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
// SteadyTime/WallDuration
///////////////////////////////////////////////////////////////////////////////////
TEST(SteadyTime, sleep){
SteadyTime start = SteadyTime::now();
WallDuration d(2.0);
bool rc = d.sleep();
SteadyTime end = SteadyTime::now();
ASSERT_GT(end - start, d);
ASSERT_TRUE(rc);
}
TEST(SteadyTime, sleepUntil){
SteadyTime start = SteadyTime::now();
SteadyTime end = start + WallDuration(2.0);
bool rc = SteadyTime::sleepUntil(end);
SteadyTime finished = SteadyTime::now();
ASSERT_GT(finished, end);
ASSERT_TRUE(rc);
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::Time::init();
return RUN_ALL_TESTS();
}