diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp index 102ab74..6e63584 100644 --- a/rclcpp/include/rclcpp/duration.hpp +++ b/rclcpp/include/rclcpp/duration.hpp @@ -89,6 +89,10 @@ public: Duration operator-(const rclcpp::Duration & rhs) const; + RCLCPP_PUBLIC + Duration + operator*(double scale) const; + RCLCPP_PUBLIC rcl_duration_value_t nanoseconds() const; diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp index b233851..eed5e8a 100644 --- a/rclcpp/src/rclcpp/duration.cpp +++ b/rclcpp/src/rclcpp/duration.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -182,6 +183,34 @@ Duration::operator-(const rclcpp::Duration & rhs) const rcl_duration_.nanoseconds - rhs.rcl_duration_.nanoseconds); } +void +bounds_check_duration_scale(int64_t dns, double scale, uint64_t max) +{ + auto abs_dns = static_cast(std::abs(dns)); + auto abs_scale = std::abs(scale); + + if (abs_scale > 1.0 && abs_dns > static_cast(max / abs_scale)) { + if ((dns > 0 && scale > 0) || (dns < 0 && scale < 0)) { + throw std::overflow_error("duration scaling leads to int64_t overflow"); + } else { + throw std::underflow_error("duration scaling leads to int64_t underflow"); + } + } +} + +Duration +Duration::operator*(double scale) const +{ + if (!std::isfinite(scale)) { + throw std::runtime_error("abnormal scale in rclcpp::Duration"); + } + bounds_check_duration_scale( + this->rcl_duration_.nanoseconds, + scale, + std::numeric_limits::max()); + return Duration(static_cast(rcl_duration_.nanoseconds * scale)); +} + rcl_duration_value_t Duration::nanoseconds() const { diff --git a/rclcpp/test/test_duration.cpp b/rclcpp/test/test_duration.cpp index 1c072da..b654212 100644 --- a/rclcpp/test/test_duration.cpp +++ b/rclcpp/test/test_duration.cpp @@ -54,6 +54,9 @@ TEST(TestDuration, operators) { EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds())); EXPECT_EQ(sub, young - old); + rclcpp::Duration scale = old * 3; + EXPECT_EQ(scale.nanoseconds(), (rcl_duration_value_t)(old.nanoseconds() * 3)); + rclcpp::Duration time = rclcpp::Duration(0, 0); rclcpp::Duration copy_constructor_duration(time); rclcpp::Duration assignment_op_duration = rclcpp::Duration(1, 0); @@ -85,4 +88,12 @@ TEST(TestDuration, overflows) { EXPECT_THROW(min - one, std::underflow_error); EXPECT_THROW(negative_one + min, std::underflow_error); EXPECT_THROW(negative_one - max, std::underflow_error); + + rclcpp::Duration base_d = max * 0.3; + EXPECT_THROW(base_d * 4, std::overflow_error); + EXPECT_THROW(base_d * (-4), std::underflow_error); + + rclcpp::Duration base_d_neg = max * (-0.3); + EXPECT_THROW(base_d_neg * (-4), std::overflow_error); + EXPECT_THROW(base_d_neg * 4, std::underflow_error); }