timer_callbacks.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
/* Author: Josh Faust */
 
/*
 * Test timer callbacks
 */
 
#include <string>
#include <sstream>
#include <fstream>
 
#include <gtest/gtest.h>
 
#include <time.h>
#include <stdlib.h>
 
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include <test_roscpp/TestArray.h>
#include <test_roscpp/TestStringString.h>
 
#include <boost/scoped_ptr.hpp>
 
using namespace ros;
using namespace test_roscpp;
 
std::string g_node_name = "test_timer_callbacks";
 
 
/************************* SteadyTimer tests **********************/
 
class SteadyTimerHelper
{
  public:
    SteadyTimerHelper(float period, bool oneshot = false)
    : expected_period_(period)
    , failed_(false)
    , total_calls_(0)
    , testing_period_(false)
    , calls_before_testing_period_(0)
    {
      NodeHandle n;
      timer_ = n.createSteadyTimer(expected_period_, &SteadyTimerHelper::callback, this, oneshot);
    }
 
    void callback(const SteadyTimerEvent& e)
    {
      bool first = last_call_.isZero();
      last_call_ = e.current_expired;
 
      if (!first)
      {
        double time_error = e.current_expired.toSec() - e.current_expected.toSec();
        // Strict check if called early, loose check if called late.
        // Yes, this is very loose, but must pass in high-load, containerized/virtualized, contentious environments.
        if (time_error > 5.0 || time_error < -0.01)
        {
          ROS_ERROR("Call came at wrong time (expected: %f, expired: %f, callback: %f)",
                    e.current_expected.toSec(), e.current_expired.toSec(), e.current_real.toSec());
          failed_ = true;
        }
      }
 
      if(testing_period_)
      {
 
        // Inside callback, less than current period, reset=false
        if(total_calls_ == calls_before_testing_period_)
        {
          WallDuration p(0.5);
          pretendWork(0.15);
          setPeriod(p);
        }
 
          // Inside callback, greater than current period, reset=false
        else if(total_calls_ == (calls_before_testing_period_+1))
        {
          WallDuration p(0.25);
          pretendWork(0.15);
          setPeriod(p);
        }
 
          // Inside callback, less than current period, reset=true
        else if(total_calls_ == (calls_before_testing_period_+2))
        {
          WallDuration p(0.5);
          pretendWork(0.15);
          setPeriod(p, true);
        }
 
          // Inside callback, greater than current period, reset=true
        else if(total_calls_ == (calls_before_testing_period_+3))
        {
          WallDuration p(0.25);
          pretendWork(0.15);
          setPeriod(p, true);
        }
      }
 
      ++total_calls_;
    }
 
    void setPeriod(const WallDuration p, bool reset=false)
    {
      timer_.setPeriod(p, reset);
      expected_period_ = p;
    }
 
 
    void pretendWork(const float t)
    {
      ros::Rate r(1. / t);
      r.sleep();
    }
 
    SteadyTime last_call_;
    WallDuration expected_period_;
 
    bool failed_;
 
    SteadyTimer timer_;
    int32_t total_calls_;
 
    bool testing_period_;
    int  calls_before_testing_period_;
};
 
TEST(RoscppTimerCallbacks, singleSteadyTimeCallback)
{
  NodeHandle n;
  SteadyTimerHelper helper1(0.01);
 
  WallDuration d(0.001f);
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    spinOnce();
    d.sleep();
  }
 
  if (helper1.failed_)
  {
    FAIL();
  }
 
  if (helper1.total_calls_ < 99)
  {
    ROS_ERROR("Total calls: %d (expected at least 100)", helper1.total_calls_);
    FAIL();
  }
}
 
TEST(RoscppTimerCallbacks, multipleSteadyTimeCallbacks)
{
  NodeHandle n;
  const int count = 100;
  typedef boost::scoped_ptr<SteadyTimerHelper> HelperPtr;
  HelperPtr helpers[count];
  for (int i = 0; i < count; ++i)
  {
    helpers[i].reset(new SteadyTimerHelper((float)(i + 1) * 0.1f));
  }
 
  WallDuration d(0.01f);
  const int spin_count = 1000;
  for (int32_t i = 0; i < spin_count && n.ok(); ++i)
  {
    spinOnce();
    d.sleep();
  }
 
  for (int i = 0; i < count; ++i)
  {
    if (helpers[i]->failed_)
    {
      ROS_ERROR("Helper %d failed", i);
      FAIL();
    }
 
    int32_t expected_count = (spin_count * d.toSec()) / helpers[i]->expected_period_.toSec();
    if (helpers[i]->total_calls_ < (expected_count - 1))
    {
      ROS_ERROR("Helper %d total calls: %d (at least %d expected)", i, helpers[i]->total_calls_, expected_count);
      FAIL();
    }
  }
}
 
TEST(RoscppTimerCallbacks, steadySetPeriod)
{
  NodeHandle n;
  WallDuration    period(0.5);
  SteadyTimerHelper helper(period.toSec());
  Rate            r(100);
 
  // Let the callback occur once before getting started
  while(helper.total_calls_ < 1)
  {
    spinOnce();
    r.sleep();
  }
 
  helper.pretendWork(0.1);
 
  // outside callback, new period < old period, reset = false
  Duration      wait(0.5);
  WallDuration  p(0.25);
  helper.setPeriod(p);
  while(helper.total_calls_ < 2)
  {
    spinOnce();
    r.sleep();
  }
 
  helper.pretendWork(0.1);
 
  // outside callback, new period > old period, reset = false
  WallDuration p2(0.5);
  helper.setPeriod(p);
  while(helper.total_calls_ < 3)
  {
    spinOnce();
    r.sleep();
  }
 
  helper.pretendWork(0.1);
 
  // outside callback, new period < old period, reset = true
  WallDuration p3(0.25);
  helper.setPeriod(p, true);
  while(helper.total_calls_ < 4)
  {
    spinOnce();
    r.sleep();
  }
 
  helper.pretendWork(0.1);
 
  // outside callback, new period > old period, reset = true
  WallDuration p4(0.5);
  helper.setPeriod(p, true);
  while(helper.total_calls_ < 5)
  {
    spinOnce();
    r.sleep();
  }
 
  // Test calling setPeriod inside callback
  helper.calls_before_testing_period_ = helper.total_calls_;
  int total = helper.total_calls_ + 5;
  helper.testing_period_ = true;
  while(helper.total_calls_ < total)
  {
    spinOnce();
    r.sleep();
  }
  helper.testing_period_ = false;
 
 
  if(helper.failed_)
  {
    ROS_ERROR("Helper failed in setPeriod");
    FAIL();
  }
}
 
TEST(RoscppTimerCallbacks, stopSteadyTimer)
{
  NodeHandle n;
  SteadyTimerHelper helper(0.001);
 
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_GT(helper.total_calls_, 0);
  int32_t last_count = helper.total_calls_;
  helper.timer_.stop();
 
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_EQ(last_count, helper.total_calls_);
}
 
int32_t g_steady_count = 0;
void steadyTimerCallback(const ros::SteadyTimerEvent&)
{
  ++g_steady_count;
}
 
TEST(RoscppTimerCallbacks, steadyStopThenSpin)
{
  g_steady_count = 0;
  NodeHandle n;
  ros::SteadyTimer timer = n.createSteadyTimer(ros::WallDuration(0.001), steadyTimerCallback);
 
  WallDuration(0.1).sleep();
  timer.stop();
 
  spinOnce();
 
  ASSERT_EQ(g_steady_count, 0);
}
 
TEST(RoscppTimerCallbacks, oneShotSteadyTimer)
{
  NodeHandle n;
  SteadyTimerHelper helper(0.001, true);
 
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_EQ(helper.total_calls_, 1);
}
 
/************************* WallTimer tests **********************/
 
class WallTimerHelper
{
public:
  WallTimerHelper(float period, bool oneshot = false)
  : expected_period_(period)
  , failed_(false)
  , total_calls_(0)
  , testing_period_(false)
  , calls_before_testing_period_(0)
  {
    NodeHandle n;
    timer_ = n.createWallTimer(expected_period_, &WallTimerHelper::callback, this, oneshot);
  }
 
  void callback(const WallTimerEvent& e)
  {
    bool first = last_call_.isZero();
    last_call_ = e.current_expired;
 
    if (!first)
    {
      double time_error = e.current_expired.toSec() - e.current_expected.toSec();
      // Strict check if called early, loose check if called late.
      // Yes, this is very loose, but must pass in high-load, containerized/virtualized, contentious environments.
      if (time_error > 5.0 || time_error < -0.01)
      {
        ROS_ERROR("Call came at wrong time (expected: %f, expired: %f, callback: %f)",
                  e.current_expected.toSec(), e.current_expired.toSec(), e.current_real.toSec());
        failed_ = true;
      }
    }
 
    if(testing_period_)
    {
 
      // Inside callback, less than current period, reset=false
      if(total_calls_ == calls_before_testing_period_)
      {
        WallDuration p(0.5);
        pretendWork(0.15);
        setPeriod(p);
      }
      
      // Inside callback, greater than current period, reset=false
      else if(total_calls_ == (calls_before_testing_period_+1))
      {
        WallDuration p(0.25);
        pretendWork(0.15);
        setPeriod(p);
      }
      
      // Inside callback, less than current period, reset=true
      else if(total_calls_ == (calls_before_testing_period_+2))
      {
        WallDuration p(0.5);
        pretendWork(0.15);
        setPeriod(p, true);
      }
      
      // Inside callback, greater than current period, reset=true
      else if(total_calls_ == (calls_before_testing_period_+3))
      {
        WallDuration p(0.25);
        pretendWork(0.15);
        setPeriod(p, true);
      }
    }
 
    ++total_calls_;
  }
 
  void setPeriod(const WallDuration p, bool reset=false)
  {
    timer_.setPeriod(p, reset);
    expected_period_ = p;
  }
 
 
  void pretendWork(const float t)
  {
    ros::Rate r(1. / t);
    r.sleep();
  }
 
  WallTime last_call_;
  WallDuration expected_period_;
 
  bool failed_;
 
  WallTimer timer_;
  int32_t total_calls_;
 
  bool testing_period_;
  int  calls_before_testing_period_;
};
 
TEST(RoscppTimerCallbacks, singleWallTimeCallback)
{
  NodeHandle n;
  WallTimerHelper helper1(0.01);
 
  WallDuration d(0.001f);
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    spinOnce();
    d.sleep();
  }
 
  if (helper1.failed_)
  {
    FAIL();
  }
 
  if (helper1.total_calls_ < 99)
  {
    ROS_ERROR("Total calls: %d (expected at least 100)", helper1.total_calls_);
    FAIL();
  }
}
 
TEST(RoscppTimerCallbacks, multipleWallTimeCallbacks)
{
  NodeHandle n;
  const int count = 100;
  typedef boost::scoped_ptr<WallTimerHelper> HelperPtr;
  HelperPtr helpers[count];
  for (int i = 0; i < count; ++i)
  {
    helpers[i].reset(new WallTimerHelper((float)(i + 1) * 0.1f));
  }
 
  WallDuration d(0.01f);
  const int spin_count = 1000;
  for (int32_t i = 0; i < spin_count && n.ok(); ++i)
  {
    spinOnce();
    d.sleep();
  }
 
  for (int i = 0; i < count; ++i)
  {
    if (helpers[i]->failed_)
    {
      ROS_ERROR("Helper %d failed", i);
      FAIL();
    }
 
    int32_t expected_count = (spin_count * d.toSec()) / helpers[i]->expected_period_.toSec();
    if (helpers[i]->total_calls_ < (expected_count - 1))
    {
      ROS_ERROR("Helper %d total calls: %d (at least %d expected)", i, helpers[i]->total_calls_, expected_count);
      FAIL();
    }
  }
}
 
TEST(RoscppTimerCallbacks, setPeriod)
{
  NodeHandle n;
  WallDuration    period(0.5);
  WallTimerHelper helper(period.toSec());
  Rate            r(100);
 
  // Let the callback occur once before getting started
  while(helper.total_calls_ < 1)
  {
    spinOnce();
    r.sleep();
  }
 
  helper.pretendWork(0.1);
  
  // outside callback, new period < old period, reset = false
  Time          start = Time::now();
  Duration      wait(0.5);
  WallDuration  p(0.25);
  helper.setPeriod(p);
  while(helper.total_calls_ < 2)
  {
    spinOnce();
    r.sleep();
  }
  
  helper.pretendWork(0.1);
  
  // outside callback, new period > old period, reset = false
  WallDuration p2(0.5);
  start = Time::now();
  helper.setPeriod(p);
  while(helper.total_calls_ < 3)
  {
    spinOnce();
    r.sleep();
  }
  
  helper.pretendWork(0.1);
  
  // outside callback, new period < old period, reset = true
  WallDuration p3(0.25);
  start = Time::now();
  helper.setPeriod(p, true);
  while(helper.total_calls_ < 4)
  {
    spinOnce();
    r.sleep();
  }
  
  helper.pretendWork(0.1);
  
  // outside callback, new period > old period, reset = true
  WallDuration p4(0.5);
  start = Time::now();
  helper.setPeriod(p, true);
  while(helper.total_calls_ < 5)
  {
    spinOnce();
    r.sleep();
  }
 
  // Test calling setPeriod inside callback
  helper.calls_before_testing_period_ = helper.total_calls_;
  int total = helper.total_calls_ + 5;
  helper.testing_period_ = true;
  while(helper.total_calls_ < total)
  {
    spinOnce();
    r.sleep();
  }
  helper.testing_period_ = false;
 
 
  if(helper.failed_)
  {
    ROS_ERROR("Helper failed in setPeriod");
    FAIL();
  }
}
 
TEST(RoscppTimerCallbacks, stopWallTimer)
{
  NodeHandle n;
  WallTimerHelper helper(0.001);
 
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_GT(helper.total_calls_, 0);
  int32_t last_count = helper.total_calls_;
  helper.timer_.stop();
 
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_EQ(last_count, helper.total_calls_);
}
 
int32_t g_count = 0;
void timerCallback(const ros::WallTimerEvent&)
{
  ++g_count;
}
 
TEST(RoscppTimerCallbacks, stopThenSpin)
{
  g_count = 0;
  NodeHandle n;
  ros::WallTimer timer = n.createWallTimer(ros::WallDuration(0.001), timerCallback);
 
  WallDuration(0.1).sleep();
  timer.stop();
 
  spinOnce();
 
  ASSERT_EQ(g_count, 0);
}
 
TEST(RoscppTimerCallbacks, oneShotWallTimer)
{
  NodeHandle n;
  WallTimerHelper helper(0.001, true);
 
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_EQ(helper.total_calls_, 1);
}
 
class TimerHelper
{
public:
  TimerHelper(Duration period, bool oneshot = false)
    : failed_(false)
    , expected_period_(period)
    , total_calls_(0)
  {
    NodeHandle n;
    timer_ = n.createTimer(expected_period_, &TimerHelper::callback, this, oneshot);
  }
 
  TimerHelper(Rate r, bool oneshot = false)
    : failed_(false)
    , expected_period_(r.expectedCycleTime())
    , total_calls_(0)
  {
    NodeHandle n;
    timer_ = n.createTimer(r, &TimerHelper::callback, this, oneshot);
  }
 
  void callback(const TimerEvent&)
  {
    ++total_calls_;
  }
 
  bool failed_;
 
  Duration expected_period_;
 
  Timer timer_;
  int32_t total_calls_;
};
 
TEST(RoscppTimerCallbacks, singleROSTimeCallback)
{
  NodeHandle n;
 
  Time now(1, 0);
  Time::setNow(now);
 
  TimerHelper helper(Duration(0, 10000000));
 
  Duration d(0, 1000000);
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    now += d;
    Time::setNow(now);
 
    while (helper.timer_.hasPending())
    {
      WallDuration(0.001).sleep();
      spinOnce();
    }
  }
 
  if (helper.failed_)
  {
    FAIL();
  }
 
  if (helper.total_calls_ != 100)
  {
    ROS_ERROR("Total calls: %d (expected 100)", helper.total_calls_);
    FAIL();
  }
}
 
TEST(RoscppTimerCallbacks, singleROSTimeCallbackFromRate)
{
  NodeHandle n;
 
  Time now(1, 0);
  Time::setNow(now);
 
  TimerHelper helper(Rate(100));
 
  Duration d(0, 1000000);
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    now += d;
    Time::setNow(now);
 
    while (helper.timer_.hasPending())
    {
      WallDuration(0.00025).sleep();
      spinOnce();
    }
  }
 
  if (helper.failed_)
  {
    FAIL();
  }
 
  if (helper.total_calls_ != 100)
  {
    ROS_ERROR("Total calls: %d (expected 100)", helper.total_calls_);
    FAIL();
  }
}
 
TEST(RoscppTimerCallbacks, oneshotROSTimer)
{
  NodeHandle n;
 
  Time now(1, 0);
  Time::setNow(now);
 
  TimerHelper helper(Duration(0, 10000000), true);
 
  Duration d(0, 1000000);
  for (int32_t i = 0; i < 1000 && n.ok(); ++i)
  {
    now += d;
    Time::setNow(now);
 
    while (helper.timer_.hasPending())
    {
      WallDuration(0.001).sleep();
      spinOnce();
    }
  }
 
  if (helper.failed_)
  {
    FAIL();
  }
 
  ASSERT_EQ(helper.total_calls_, 1);
}
 
TEST(RoscppTimerCallbacks, singleROSTimeCallbackLargeTimestep)
{
  NodeHandle n;
 
  Time now(1, 0);
  Time::setNow(now);
 
  TimerHelper helper(Duration(0, 10000000));
 
  Duration d(0, 100000000);
  for (int32_t i = 0; i < 100 && n.ok(); ++i)
  {
    now += d;
    Time::setNow(now);
 
    while (helper.timer_.hasPending())
    {
      WallDuration(0.001).sleep();
      spinOnce();
    }
  }
 
  if (helper.failed_)
  {
    FAIL();
  }
 
  if (helper.total_calls_ != 200)
  {
    ROS_ERROR("Total calls: %d (expected 200)", helper.total_calls_);
    FAIL();
  }
}
 
TEST(RoscppTimerCallbacks, multipleROSTimeCallbacks)
{
  NodeHandle n;
 
  Time now(1, 0);
  Time::setNow(now);
 
  const int count = 100;
  typedef boost::scoped_ptr<TimerHelper> HelperPtr;
  HelperPtr helpers[count];
  for (int i = 0; i < count; ++i)
  {
    helpers[i].reset(new TimerHelper(Duration((float)(i + 1) * 0.01f)));
  }
 
  Duration d(0.001f);
  const int spin_count = 1000;
  for (int32_t i = 0; i < spin_count && n.ok(); ++i)
  {
    now += d;
    Time::setNow(now);
 
    bool pending = false;
 
    do
    {
      pending = false;
      for (int i = 0; i < count; ++i)
      {
        pending |= helpers[i]->timer_.hasPending();
      }
 
      WallDuration(0.001).sleep();
      spinOnce();
    } while (pending);
  }
 
  for (int i = 0; i < count; ++i)
  {
    if (helpers[i]->failed_)
    {
      ROS_ERROR("Helper %d failed", i);
      FAIL();
    }
 
    int32_t expected_count = (spin_count * d.toSec()) / helpers[i]->expected_period_.toSec();
    if (helpers[i]->total_calls_ < (expected_count - 1) || helpers[i]->total_calls_ > (expected_count + 1))
    {
      ROS_ERROR("Helper %d total calls: %d (%d expected)", i, helpers[i]->total_calls_, expected_count);
      FAIL();
    }
  }
}
 
class Tracked
{
public:
  Tracked()
  {
    g_count = 0;
  }
 
  void callback(const TimerEvent&)
  {
    ++g_count;
  }
};
 
TEST(RoscppTimerCallbacks, trackedObject)
{
  NodeHandle n;
  Time now(1, 0);
  Time::setNow(now);
 
  boost::shared_ptr<Tracked> tracked(boost::make_shared<Tracked>());
  Timer timer = n.createTimer(Duration(0.001), &Tracked::callback, tracked);
 
  now += Duration(0.1);
  Time::setNow(now);
 
  while (timer.hasPending())
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_GT(g_count, 0);
  int32_t last_count = g_count;
  tracked.reset();
 
  now += Duration(0.1);
  Time::setNow(now);
 
  while (timer.hasPending())
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_EQ(last_count, g_count);
}
 
TEST(RoscppTimerCallbacks, stopROSTimer)
{
  NodeHandle n;
  Time now(1, 0);
  Time::setNow(now);
 
  TimerHelper helper(Duration(0.001));
 
  now += Duration(0.1);
  Time::setNow(now);
 
  while (helper.timer_.hasPending())
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_GT(helper.total_calls_, 0);
  int32_t last_count = helper.total_calls_;
  helper.timer_.stop();
 
  now += Duration(0.1);
  Time::setNow(now);
 
  while (helper.timer_.hasPending())
  {
    WallDuration(0.001).sleep();
    spinOnce();
  }
 
  ASSERT_EQ(last_count, helper.total_calls_);
}
 
int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::init(argc, argv, g_node_name);
 
  return RUN_ALL_TESTS();
}