Make rclcpp::Duration support scale operation

Duration scale is a convinient operation which had supported in ROS.
This commit make ROS2 support it.

Signed-off-by: jwang <jing.j.wang@intel.com>

rclcpp/include/rclcpp/duration.hpp

@@ -89,6 +89,10 @@ class Duration
   Duration
   operator-(const rclcpp::Duration & rhs) const;
 
+  RCLCPP_PUBLIC
+  Duration
+  operator*(double scale) const;
+
   RCLCPP_PUBLIC
   rcl_duration_value_t
   nanoseconds() const;

rclcpp/src/rclcpp/duration.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <cmath>
 #include <cstdlib>
 #include <limits>
 #include <utility>
@@ -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<uint64_t>(std::abs(dns));
+  auto abs_scale = std::abs(scale);
+
+  if (abs_scale > 1.0 && abs_dns > static_cast<uint64_t>(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<rcl_duration_value_t>::max());
+  return Duration(static_cast<rcl_duration_value_t>(rcl_duration_.nanoseconds * scale));
+}
+
 rcl_duration_value_t
 Duration::nanoseconds() const
 {

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);
 }