niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Make rclcpp::Duration support scale operation

Browse source 
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>
This commit is contained in:
jwang 2018-02-14 03:15:28 +08:00 • committed by Tully Foote
parent d8abea55ec
commit af6e86c522
3 changed files with 44 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

4
rclcpp/include/rclcpp/duration.hpp
View file

@ -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;

29
rclcpp/src/rclcpp/duration.cpp
View file

@ -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
{

11
rclcpp/test/test_duration.cpp
View file

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