Gazebo Plugin ROS Time API call can result in exception when /use_sim_time is true

[josephcoombe]

An example of #102

---

I am experiencing frequent (non-deterministic) crashes when simulating a rather large model containing 8 HEBI 6-DOF arms in Gazebo.

Here's a relevant snippet of the terminal output:

[ INFO] [1536678807.688364589]: Loaded hebiros gazebo plugin
[ INFO] [1536678813.160947032, 202.112000000]: Created group [arm_5]:
[ INFO] [1536678813.160990233, 202.112000000]: /arm_5/Arm5/Base
[ INFO] [1536678813.161012166, 202.112000000]: /arm_5/Arm5/Shoulder
[ INFO] [1536678813.161025133, 202.112000000]: /arm_5/Arm5/Elbow
[ INFO] [1536678813.161040668, 202.112000000]: /arm_5/Arm5/Wrist1
[ INFO] [1536678813.161058963, 202.112000000]: /arm_5/Arm5/Wrist2
[ INFO] [1536678813.161070203, 202.112000000]: /arm_5/Arm5/Wrist3
[ INFO] [1536678813.198664727, 202.120000000]: current_time: 202.120000, prev_time: 3263145632.000033
terminate called after throwing an instance of 'std::runtime_error'
  what():  Duration is out of dual 32-bit range

Thread 37 "gzserver" received signal SIGABRT, Aborted.
[Switching to Thread 0x7fff5bfff700 (LWP 18846)]
0x00007ffff6b1d428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
54	../sysdeps/unix/sysv/linux/raise.c: No such file or directory.

The line

[ INFO] [1536678813.198664727, 202.120000000]: current_time: 202.120000, prev_time: 3263145632.000033

comes from adding a debug statement to HebirosGazeboPlugin::OnUpdate method:

//Update the joints at every simulation iteration
void HebirosGazeboPlugin::OnUpdate(const common::UpdateInfo & _info) {

  ros::Time current_time = ros::Time::now();

  for (auto group_pair : hebiros_groups) {
    auto hebiros_group = group_pair.second;

    // Get the time elapsed since the last iteration
    ROS_INFO("current_time: %f, prev_time: %f", current_time.toSec(), hebiros_group->prev_time.toSec());
    ros::Duration iteration_time = current_time - hebiros_group->prev_time;
    hebiros_group->prev_time = current_time;
    if (hebiros_group->group_added) {
      UpdateGroup(hebiros_group, iteration_time);
    }
  }
}

---

See this related ROS Answer: https://answers.ros.org/question/209452/exception-thrown-while-processing-service-call-time-is-out-of-dual-32-bit-range/?answer=210134#post-id-210134

[iamtesch]

Fixed by #104 (thanks @josephcoombe !)