diff --git a/rostime/include/ros/impl/duration.h b/rostime/include/ros/impl/duration.h index 7ebd202a..967bf2fc 100644 --- a/rostime/include/ros/impl/duration.h +++ b/rostime/include/ros/impl/duration.h @@ -63,11 +63,14 @@ namespace ros { template T& DurationBase::fromNSec(int64_t t) { - int64_t sec64 = t / 1000000000; + int64_t sec64 = t / 1000000000LL; if (sec64 < INT_MIN || sec64 > INT_MAX) throw std::runtime_error("Duration is out of dual 32-bit range"); sec = (int32_t)sec64; - nsec = (int32_t)(t % 1000000000); + nsec = (int32_t)(t % 1000000000LL); + + normalizeSecNSecSigned(sec, nsec); + return *static_cast(this); } diff --git a/rostime/test/time.cpp b/rostime/test/time.cpp index 93ffd539..5c837d68 100644 --- a/rostime/test/time.cpp +++ b/rostime/test/time.cpp @@ -415,6 +415,21 @@ TEST(Duration, ToFromSec) 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) { @@ -448,6 +463,13 @@ TEST(Duration, OperatorMinus) } + 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)