Merge pull request avidbots#49 from avidbots/catch-time-exceptions

Catch runtime exceptions when adding/subtracting times

flatland_plugins/src/bumper.cpp

@@ -131,7 +131,7 @@ void Bumper::AfterPhysicsStep(const Timekeeper &timekeeper) {
 
   flatland_msgs::Collisions collisions;
   collisions.header.frame_id = world_frame_id_;
-  collisions.header.stamp = ros::Time::now();
+  collisions.header.stamp = timekeeper.GetSimTime();
 
   // loop through all collisions in our record and publish
   for (it = contact_states_.begin(); it != contact_states_.end(); it++) {

flatland_plugins/src/diff_drive.cpp

@@ -174,7 +174,7 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
         b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0));
     float angular_vel = b2body->GetAngularVelocity();
 
-    ground_truth_msg_.header.stamp = ros::Time::now();
+    ground_truth_msg_.header.stamp = timekeeper.GetSimTime();
     ground_truth_msg_.pose.pose.position.x = position.x;
     ground_truth_msg_.pose.pose.position.y = position.y;
     ground_truth_msg_.pose.pose.position.z = 0;
@@ -188,7 +188,7 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
     ground_truth_msg_.twist.twist.angular.z = angular_vel;
 
     // add the noise to odom messages
-    odom_msg_.header.stamp = ros::Time::now();
+    odom_msg_.header.stamp = timekeeper.GetSimTime();
     odom_msg_.pose.pose = ground_truth_msg_.pose.pose;
     odom_msg_.twist.twist = ground_truth_msg_.twist.twist;
     odom_msg_.pose.pose.position.x += noise_gen_[0](rng_);
@@ -208,7 +208,7 @@ void DiffDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
       // Transform global frame velocity into local frame to simulate encoder
       // readings
       geometry_msgs::TwistStamped twist_pub_msg;
-      twist_pub_msg.header.stamp = ros::Time::now();
+      twist_pub_msg.header.stamp = timekeeper.GetSimTime();
       twist_pub_msg.header.frame_id = odom_msg_.child_frame_id;
 
       // Forward velocity in twist.linear.x

flatland_plugins/src/gps.cpp

@@ -28,12 +28,12 @@ void Gps::BeforePhysicsStep(const Timekeeper &timekeeper) {
   // only compute and publish when the number of subscribers is not zero
   if (fix_publisher_.getNumSubscribers() > 0) {
     UpdateFix();
-    gps_fix_.header.stamp = ros::Time::now();
+    gps_fix_.header.stamp = timekeeper.GetSimTime();
     fix_publisher_.publish(gps_fix_);
   }
 
   if (broadcast_tf_) {
-    gps_tf_.header.stamp = ros::Time::now();
+    gps_tf_.header.stamp = timekeeper.GetSimTime();
     tf_broadcaster_.sendTransform(gps_tf_);
   }
 }

flatland_plugins/src/laser.cpp

@@ -136,12 +136,12 @@ void Laser::BeforePhysicsStep(const Timekeeper &timekeeper) {
   // only compute and publish when the number of subscribers is not zero
   if (scan_publisher_.getNumSubscribers() > 0) {
     ComputeLaserRanges();
-    laser_scan_.header.stamp = ros::Time::now();
+    laser_scan_.header.stamp = timekeeper.GetSimTime();
     scan_publisher_.publish(laser_scan_);
   }
 
   if (broadcast_tf_) {
-    laser_tf_.header.stamp = ros::Time::now();
+    laser_tf_.header.stamp = timekeeper.GetSimTime();
     tf_broadcaster_.sendTransform(laser_tf_);
   }
 }

flatland_plugins/src/model_tf_publisher.cpp

@@ -119,7 +119,7 @@ void ModelTfPublisher::BeforePhysicsStep(const Timekeeper &timekeeper) {
   ref_tf_m << r.q.c, -r.q.s, r.p.x, r.q.s, r.q.c, r.p.y, 0, 0, 1;
 
   geometry_msgs::TransformStamped tf_stamped;
-  tf_stamped.header.stamp = ros::Time::now();
+  tf_stamped.header.stamp = timekeeper.GetSimTime();
 
   // loop through the bodies to calculate TF, and ignores excluded bodies
   for (unsigned int i = 0; i < GetModel()->bodies_.size(); i++) {

flatland_plugins/src/tricycle_drive.cpp

@@ -296,7 +296,7 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
         b2body->GetLinearVelocityFromLocalPoint(b2Vec2(0, 0));
     float angular_vel = b2body->GetAngularVelocity();
 
-    ground_truth_msg_.header.stamp = ros::Time::now();
+    ground_truth_msg_.header.stamp = timekeeper.GetSimTime();
     ground_truth_msg_.pose.pose.position.x = position.x;
     ground_truth_msg_.pose.pose.position.y = position.y;
     ground_truth_msg_.pose.pose.position.z = 0;
@@ -310,7 +310,7 @@ void TricycleDrive::BeforePhysicsStep(const Timekeeper& timekeeper) {
     ground_truth_msg_.twist.twist.angular.z = angular_vel;
 
     // add the noise to odom messages
-    odom_msg_.header.stamp = ros::Time::now();
+    odom_msg_.header.stamp = timekeeper.GetSimTime();
     odom_msg_.pose.pose = ground_truth_msg_.pose.pose;
     odom_msg_.twist.twist = ground_truth_msg_.twist.twist;
     odom_msg_.pose.pose.position.x += noise_gen_[0](rng_);

flatland_server/include/flatland_server/debug_visualization.h

@@ -56,6 +56,7 @@
 #include <vector>
 
 #include "flatland_server/body.h"
+#include "flatland_server/timekeeper.h"
 
 namespace flatland_server {
 struct DebugTopic {
@@ -80,8 +81,9 @@ class DebugVisualization {
 
   /**
    * @brief Publish all marker array topics_ that need publishing
+   * @param[in] timekeeper The time object to use for header timestamps
    */
-  void Publish();
+  void Publish(const Timekeeper& timekeeper);
 
   /**
    * @brief Visualize body

flatland_server/src/debug_visualization.cpp

@@ -78,7 +78,6 @@ void DebugVisualization::JointToMarkers(
 
   visualization_msgs::Marker marker;
   marker.header.frame_id = "map";
-  marker.header.stamp = ros::Time::now();
   marker.color.r = r;
   marker.color.g = g;
   marker.color.b = b;
@@ -127,7 +126,6 @@ void DebugVisualization::BodyToMarkers(visualization_msgs::MarkerArray& markers,
   while (fixture != NULL) {  // traverse fixture linked list
     visualization_msgs::Marker marker;
     marker.header.frame_id = "map";
-    marker.header.stamp = ros::Time::now();
     marker.id = markers.markers.size();
     marker.color.r = r;
     marker.color.g = g;
@@ -218,7 +216,7 @@ void DebugVisualization::BodyToMarkers(visualization_msgs::MarkerArray& markers,
   }
 }
 
-void DebugVisualization::Publish() {
+void DebugVisualization::Publish(const Timekeeper& timekeeper) {
   // Iterate over the topics_ map as pair(name, topic)
 
   std::vector<std::string> to_delete;
@@ -233,6 +231,10 @@ void DebugVisualization::Publish() {
     if (topic.second.markers.markers.size() == 0) {
       to_delete.push_back(topic.first);
     } else {
+      // Iterate the marker array to update all the timestamps
+      for (unsigned int i = 0; i < topic.second.markers.markers.size(); i++) {
+        topic.second.markers.markers[i].header.stamp = timekeeper.GetSimTime();
+      }
       topic.second.publisher.publish(topic.second.markers);
       topic.second.needs_publishing = false;
     }
@@ -258,7 +260,6 @@ void DebugVisualization::VisualizeLayer(std::string name, Body* body) {
   while (fixture != NULL) {  // traverse fixture linked list
 
     marker.header.frame_id = "map";
-    marker.header.stamp = ros::Time::now();
     marker.id = topics_[name].markers.markers.size();
     marker.color.r = body->color_.r;
     marker.color.g = body->color_.g;

flatland_server/src/interactive_marker_manager.cpp

@@ -83,6 +83,7 @@ void InteractiveMarkerManager::createInteractiveMarker(
     // Transform original body frame marker from global to local frame
     RotateTranslate rt = Geometry::CreateTransform(pose.x, pose.y, pose.theta);
     transformed_body_marker.header.frame_id = "";
+    transformed_body_marker.header.stamp = ros::Time(0);
     transformed_body_marker.pose.position.x =
         (body_markers.markers[i].pose.position.x - rt.dx) * rt.cos +
         (body_markers.markers[i].pose.position.y - rt.dy) * rt.sin;
@@ -108,6 +109,7 @@ void InteractiveMarkerManager::createInteractiveMarker(
   // Send new interactive marker to server
   visualization_msgs::InteractiveMarker new_interactive_marker;
   new_interactive_marker.header.frame_id = "map";
+  new_interactive_marker.header.stamp = ros::Time(0);
   new_interactive_marker.name = model_name;
   new_interactive_marker.pose.position.x = pose.x;
   new_interactive_marker.pose.position.y = pose.y;
@@ -227,7 +229,14 @@ void InteractiveMarkerManager::update() {
   // in at 33 Hz if the user is dragging the marker.  When the stream of
   // pose update feedback stops, automatically clear the manipulating_model_
   // flag to unpause the simulation.
-  double dt = (ros::WallTime::now() - pose_update_stamp_).toSec();
+  double dt = 0;
+  try {
+    dt = (ros::WallTime::now() - pose_update_stamp_).toSec();
+  } catch (std::runtime_error &ex) {
+    ROS_ERROR(
+        "Flatland Interactive Marker Manager runtime error: (%f - %f) [%s]",
+        ros::WallTime::now().toSec(), pose_update_stamp_.toSec(), ex.what());
+  }
   if (manipulating_model_ && dt > 0.1 && dt < 1.0) {
     manipulating_model_ = false;
   }

flatland_server/src/simulation_manager.cpp

@@ -80,7 +80,7 @@ void SimulationManager::Main() {
   try {
     world_ = World::MakeWorld(world_yaml_file_);
     ROS_INFO_NAMED("SimMan", "World loaded");
-  } catch (const std::exception &e) {
+  } catch (const std::exception& e) {
     ROS_FATAL_NAMED("SimMan", "%s", e.what());
     return;
   }
@@ -102,16 +102,22 @@ void SimulationManager::Main() {
   while (ros::ok() && run_simulator_) {
     // for updating visualization at a given rate
     // see flatland_plugins/update_timer.cpp for this formula
-    double f = fmod(
-        ros::WallTime::now().toSec() + (rate.expectedCycleTime().toSec() / 2.0),
-        viz_update_period);
+    double f = 0.0;
+    try {
+      f = fmod(ros::WallTime::now().toSec() +
+                   (rate.expectedCycleTime().toSec() / 2.0),
+               viz_update_period);
+    } catch (std::runtime_error& ex) {
+      ROS_ERROR("Flatland runtime error: [%s]", ex.what());
+    }
     bool update_viz = ((f >= 0.0) && (f < rate.expectedCycleTime().toSec()));
 
     world_->Update(timekeeper);  // Step physics by ros cycle time
 
     if (show_viz_ && update_viz) {
-      world_->DebugVisualize(false);        // no need to update layer
-      DebugVisualization::Get().Publish();  // publish debug visualization
+      world_->DebugVisualize(false);  // no need to update layer
+      DebugVisualization::Get().Publish(
+          timekeeper);  // publish debug visualization
     }
 
     ros::spinOnce();

flatland_server/test/debug_visualization_test.cpp

@@ -46,6 +46,7 @@
 
 #include "flatland_server/debug_visualization.h"
 #include <Box2D/Box2D.h>
+#include <flatland_server/timekeeper.h>
 #include <gtest/gtest.h>
 #include <ros/ros.h>
 #include <visualization_msgs/MarkerArray.h>
@@ -78,8 +79,8 @@ TEST(DebugVizTest, testBodyToMarkersPolygon) {
 
   // Check that
   ASSERT_EQ(markers.markers[0].header.frame_id, "map");
-  ASSERT_NE(markers.markers[0].header.stamp.sec, 0);
-  ASSERT_NE(markers.markers[0].header.stamp.nsec, 0);
+  ASSERT_EQ(markers.markers[0].header.stamp.sec, 0);
+  ASSERT_EQ(markers.markers[0].header.stamp.nsec, 0);
 
   // Check color setting
   ASSERT_NEAR(markers.markers[0].color.r, 1.0, 1e-5);
@@ -410,6 +411,9 @@ struct MarkerArraySubscriptionHelper {
 
 // Test the bodyToMarkers method on an unsupported shape
 TEST(DebugVizTest, testPublishMarkers) {
+  flatland_server::Timekeeper timekeeper;
+  timekeeper.SetMaxStepSize(0.01);
+
   b2Vec2 gravity(0.0, 0.0);
   b2World world(gravity);
 
@@ -454,14 +458,14 @@ TEST(DebugVizTest, testPublishMarkers) {
   EXPECT_EQ(sub.getNumPublishers(), 1);
 
   // Publish
-  flatland_server::DebugVisualization::Get().Publish();
+  flatland_server::DebugVisualization::Get().Publish(timekeeper);
 
   // Verify that message was published
   EXPECT_TRUE(helper.waitForMessageCount(1));
   EXPECT_EQ(helper.markers_.markers.size(), 1);
 
   // Publish again (should have no change- nothing needs publishing)
-  flatland_server::DebugVisualization::Get().Publish();
+  flatland_server::DebugVisualization::Get().Publish(timekeeper);
 
   // Verify that message was published
   EXPECT_TRUE(helper.waitForMessageCount(1));
@@ -475,7 +479,7 @@ TEST(DebugVizTest, testPublishMarkers) {
   // inserts two markers
   flatland_server::DebugVisualization::Get().Visualize("example", joint, 1.0,
                                                        0.0, 0.0, 1.0);
-  flatland_server::DebugVisualization::Get().Publish();
+  flatland_server::DebugVisualization::Get().Publish(timekeeper);
 
   // Verify that message was published
   EXPECT_TRUE(helper.waitForMessageCount(2));    // Published twice
@@ -484,15 +488,15 @@ TEST(DebugVizTest, testPublishMarkers) {
   // Reset marker list, this empties the markers array, and topics having
   // empty markers are automatically deleted
   flatland_server::DebugVisualization::Get().Reset("example");
-  flatland_server::DebugVisualization::Get().Publish();
+  flatland_server::DebugVisualization::Get().Publish(timekeeper);
 
   // Verify that message was published
   EXPECT_TRUE(helper.waitForMessageCount(2));  // Published two times
 
   // publish again with some contents, and the topic is created again
   flatland_server::DebugVisualization::Get().Visualize("example", joint, 1.0,
                                                        0.0, 0.0, 1.0);
-  flatland_server::DebugVisualization::Get().Publish();
+  flatland_server::DebugVisualization::Get().Publish(timekeeper);
 
   EXPECT_TRUE(helper.waitForMessageCount(3));
   EXPECT_EQ(2, helper.markers_.markers.size());

flatland_viz/include/flatland_viz/flatland_window.h

@@ -91,17 +91,10 @@ class FlatlandWindow : public QMainWindow {
 
   rviz::VisualizationManager* getManager();
 
-  QLabel* fps_label_;
-
  protected Q_SLOTS:
 
   void openNewToolDialog();
 
-  void UpdateFps();
-
  private:
   FlatlandViz* viz_;
-
-  int frame_count_;
-  ros::WallTime last_fps_calc_time_;
 };

flatland_viz/src/flatland_window.cpp

@@ -108,30 +108,6 @@ FlatlandWindow::FlatlandWindow(QWidget *parent) : QMainWindow(parent) {
   setCentralWidget(viz_);
   resize(QDesktopWidget().availableGeometry(this).size() * 0.9);
 
-  // Configure the status bar
-  fps_label_ = new QLabel("");
-  fps_label_->setMinimumWidth(40);
-  fps_label_->setAlignment(Qt::AlignRight);
-  statusBar()->addPermanentWidget(fps_label_, 0);
-
   // Set the main window properties
   setWindowTitle("Flatland Viz");
-  QFont font;
-  font.setPointSize(font.pointSizeF() * 0.9);
-
-  connect(viz_->manager_, &rviz::VisualizationManager::preUpdate, this,
-          &FlatlandWindow::UpdateFps);
-}
-
-void FlatlandWindow::UpdateFps() {
-  frame_count_++;
-  ros::WallDuration wall_diff = ros::WallTime::now() - last_fps_calc_time_;
-
-  if (wall_diff.toSec() > 1.0) {
-    float fps = frame_count_ / wall_diff.toSec();
-    frame_count_ = 0;
-    last_fps_calc_time_ = ros::WallTime::now();
-
-    fps_label_->setText(QString::number(int(fps)) + QString(" fps"));
-  }
 }