tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ros: ros timestamp test

Browse Source
This commit is contained in:
kalman 2019-06-28 10:55:57 +08:00
parent ab6a9b03a3
commit a7d97427f5
2 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch
View File

@ -48,7 +48,7 @@
<group ns="$(arg mynteye)"> <group ns="$(arg mynteye)">
<!-- mynteye_wrapper_node --> <!-- mynteye_wrapper_node -->
<node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="true" respawn_delay="10"> <node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="false" respawn_delay="10">
<!-- node params --> <!-- node params -->

15
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -58,6 +58,7 @@ inline double compute_time(const double end, const double start) {
class ROSWrapperNodelet : public nodelet::Nodelet { class ROSWrapperNodelet : public nodelet::Nodelet {
public: public:
ROSWrapperNodelet() { ROSWrapperNodelet() {
unit_hard_time *= 10;
} }
~ROSWrapperNodelet() { ~ROSWrapperNodelet() {
@ -140,10 +141,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
inline bool is_overflow(std::uint64_t now, inline bool is_overflow(std::uint64_t now,
std::uint64_t pre) { std::uint64_t pre) {
static std::uint64_t unit =
std::numeric_limits<std::uint32_t>::max();
return (now < pre) && ((pre - now) > (unit / 2)); return (now < pre) && ((pre - now) > (unit_hard_time / 2));
} }
inline bool is_repeated(std::uint64_t now, inline bool is_repeated(std::uint64_t now,
@ -153,18 +152,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
inline bool is_abnormal(std::uint32_t now, inline bool is_abnormal(std::uint32_t now,
std::uint32_t pre) { std::uint32_t pre) {
static std::uint64_t unit =
std::numeric_limits<std::uint32_t>::max();
return (now < pre) && ((pre - now) < (unit / 4)); return (now < pre) && ((pre - now) < (unit_hard_time / 4));
} }
ros::Time checkUpTimeStamp(std::uint64_t _hard_time, ros::Time checkUpTimeStamp(std::uint64_t _hard_time,
const Stream &stream) { const Stream &stream) {
static std::map<Stream, std::uint64_t> hard_time_now; static std::map<Stream, std::uint64_t> hard_time_now;
static std::map<Stream, std::uint64_t> acc; static std::map<Stream, std::uint64_t> acc;
static std::uint64_t unit_hard_time =
std::numeric_limits<std::uint32_t>::max();
if (is_overflow(_hard_time, hard_time_now[stream])) { if (is_overflow(_hard_time, hard_time_now[stream])) {
acc[stream]++; acc[stream]++;
@ -178,8 +173,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
ros::Time checkUpImuTimeStamp(std::uint64_t _hard_time) { ros::Time checkUpImuTimeStamp(std::uint64_t _hard_time) {
static std::uint64_t hard_time_now(0), acc(0); static std::uint64_t hard_time_now(0), acc(0);
static std::uint64_t unit_hard_time =
std::numeric_limits<std::uint32_t>::max();
if (is_overflow(_hard_time, hard_time_now)) { if (is_overflow(_hard_time, hard_time_now)) {
acc++; acc++;
@ -1606,6 +1599,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
int frame_rate_; int frame_rate_;
bool is_intrinsics_enable_; bool is_intrinsics_enable_;
std::vector<ImuData> imu_align_; std::vector<ImuData> imu_align_;
std::uint64_t unit_hard_time = std::numeric_limits<std::uint32_t>::max();
}; };
MYNTEYE_END_NAMESPACE MYNTEYE_END_NAMESPACE