Removed getTimestamp override from F200 and SR300

DerelictRobot · Mar 3, 2017 · 33b08a4 · 33b08a4
1 parent 7b07bb2
commit 33b08a4
Show file tree

Hide file tree

Showing 6 changed files with 3 additions and 33 deletions.
diff --git a/realsense_camera/include/realsense_camera/f200_nodelet.h b/realsense_camera/include/realsense_camera/f200_nodelet.h
@@ -53,7 +53,6 @@ class F200Nodelet: public realsense_camera::SyncNodelet
 
  // Member Functions.
  void setStreams();
- ros::Time getTimestamp(rs_stream stream_index, double frame_ts);
  std::vector<std::string> setDynamicReconfServer();
  void startDynamicReconfCallback();
  void configCallback(realsense_camera::f200_paramsConfig &config, uint32_t level);

diff --git a/realsense_camera/include/realsense_camera/sr300_nodelet.h b/realsense_camera/include/realsense_camera/sr300_nodelet.h
@@ -54,7 +54,6 @@ class SR300Nodelet: public realsense_camera::SyncNodelet
 
  // Member Functions.
  void setStreams();
- ros::Time getTimestamp(rs_stream stream_index, double frame_ts);
  std::vector<std::string> setDynamicReconfServer();
  void startDynamicReconfCallback();
  void configCallback(realsense_camera::sr300_paramsConfig &config, uint32_t level);

diff --git a/realsense_camera/include/realsense_camera/sync_nodelet.h b/realsense_camera/include/realsense_camera/sync_nodelet.h
@@ -46,6 +46,7 @@ class SyncNodelet: public realsense_camera::BaseNodelet
 protected:
  boost::shared_ptr<boost::thread> topic_thread_;
  bool duplicate_depth_color_ = false;
+ ros::Time topic_ts_;
 
  virtual void setFrameCallbacks() { } // don't set callbacks!
  virtual void publishSyncTopics();

diff --git a/realsense_camera/src/f200_nodelet.cpp b/realsense_camera/src/f200_nodelet.cpp
@@ -82,21 +82,6 @@ namespace realsense_camera
  }
  }
 
- /*
- * Determine the timestamp for the publish topic. -- overrides base class
- */
- ros::Time F200Nodelet::getTimestamp(rs_stream stream_index, double frame_ts)
- {
- static ros::Time last_common_stamp = ros::Time::now();
-
- if (stream_index == fastest_stream_)
- {
- last_common_stamp = ros::Time::now();
- }
-
- return last_common_stamp;
- }
-
  /*
  * Set Dynamic Reconfigure Server and return the dynamic params.
  */

diff --git a/realsense_camera/src/sr300_nodelet.cpp b/realsense_camera/src/sr300_nodelet.cpp
@@ -81,21 +81,6 @@ namespace realsense_camera
  }
  }
 
- /*
- * Determine the timestamp for the publish topic. -- overrides base class
- */
- ros::Time SR300Nodelet::getTimestamp(rs_stream stream_index, double frame_ts)
- {
- static ros::Time last_common_stamp = ros::Time::now();
-
- if (stream_index == fastest_stream_)
- {
- last_common_stamp = ros::Time::now();
- }
-
- return last_common_stamp;
- }
-
  /*
  * Set Dynamic Reconfigure Server and return the dynamic params.
  */

diff --git a/realsense_camera/src/sync_nodelet.cpp b/realsense_camera/src/sync_nodelet.cpp
@@ -54,6 +54,7 @@ namespace realsense_camera
  {
  rs_wait_for_frames(rs_device_, &rs_error_);
  checkError();
+ topic_ts_ = ros::Time::now();
 
  for (int stream=0; stream < STREAM_COUNT; stream++)
  {
@@ -117,7 +118,7 @@ namespace realsense_camera
  image_[stream_index]).toImageMsg();
 
  msg->header.frame_id = optical_frame_id_[stream_index];
- msg->header.stamp = getTimestamp(stream_index, frame_ts);; // Publish timestamp to synchronize frames.
+ msg->header.stamp = topic_ts_; // Publish timestamp to synchronize frames.
  msg->width = image_[stream_index].cols;
  msg->height = image_[stream_index].rows;
  msg->is_bigendian = false;