Merge pull request IntelRealSense#209 from mkhansen-intel/sync_F200_S…

…R300

Revert to wait for frame for R200, F200, SR300 cameras

realsense_camera/CMakeLists.txt

@@ -101,7 +101,7 @@ include_directories(
  ${catkin_INCLUDE_DIRS}
 )
 
-add_library(${PROJECT_NAME}_nodelet src/base_nodelet.cpp src/r200_nodelet.cpp src/f200_nodelet.cpp src/sr300_nodelet.cpp
+add_library(${PROJECT_NAME}_nodelet src/base_nodelet.cpp src/sync_nodelet.cpp src/r200_nodelet.cpp src/f200_nodelet.cpp src/sr300_nodelet.cpp
  src/zr300_nodelet.cpp)
 target_link_libraries(${PROJECT_NAME}_nodelet
  ${catkin_LIBRARIES}

realsense_camera/include/realsense_camera/base_nodelet.h

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -139,6 +139,8 @@ class BaseNodelet: public nodelet::Nodelet
  rs_extrinsics color2depth_extrinsic_; // color frame is base frame
  rs_extrinsics color2ir_extrinsic_; // color frame is base frame
  rs_source rs_source_ = RS_SOURCE_VIDEO;
+ bool start_camera_ = true;
+ bool start_stop_srv_called_ = false;
 
  struct CameraOptions
  {

realsense_camera/include/realsense_camera/f200_nodelet.h

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -37,11 +37,11 @@
 #include <dynamic_reconfigure/server.h>
 
 #include <realsense_camera/f200_paramsConfig.h>
-#include <realsense_camera/base_nodelet.h>
+#include <realsense_camera/sync_nodelet.h>
 
 namespace realsense_camera
 {
-class F200Nodelet: public realsense_camera::BaseNodelet
+class F200Nodelet: public realsense_camera::SyncNodelet
 {
 public:
  void onInit();
@@ -53,7 +53,6 @@ class F200Nodelet: public realsense_camera::BaseNodelet
 
  // 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);

realsense_camera/include/realsense_camera/r200_nodelet.h

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -38,11 +38,11 @@
 #include <dynamic_reconfigure/server.h>
 
 #include <realsense_camera/r200_paramsConfig.h>
-#include <realsense_camera/base_nodelet.h>
+#include <realsense_camera/sync_nodelet.h>
 
 namespace realsense_camera
 {
-class R200Nodelet: public realsense_camera::BaseNodelet
+class R200Nodelet: public realsense_camera::SyncNodelet
 {
 public:
  void onInit();
@@ -71,8 +71,6 @@ class R200Nodelet: public realsense_camera::BaseNodelet
  void getCameraExtrinsics();
  void publishStaticTransforms();
  void publishDynamicTransforms();
- void setFrameCallbacks();
- std::function<void(rs::frame f)> ir2_frame_handler_;
 };
 } // namespace realsense_camera
 #endif // REALSENSE_CAMERA_R200_NODELET_H

realsense_camera/include/realsense_camera/sr300_nodelet.h

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -38,11 +38,11 @@
 #include <dynamic_reconfigure/server.h>
 
 #include <realsense_camera/sr300_paramsConfig.h>
-#include <realsense_camera/base_nodelet.h>
+#include <realsense_camera/sync_nodelet.h>
 
 namespace realsense_camera
 {
-class SR300Nodelet: public realsense_camera::BaseNodelet
+class SR300Nodelet: public realsense_camera::SyncNodelet
 {
 public:
  void onInit();
@@ -54,7 +54,6 @@ class SR300Nodelet: public realsense_camera::BaseNodelet
 
  // 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);

realsense_camera/include/realsense_camera/sync_nodelet.h

@@ -0,0 +1,58 @@
+/******************************************************************************
+ Copyright (c) 2017, Intel Corporation
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without
+ modification, are permitted provided that the following conditions are met:
+
+ 1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+
+ 2. 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.
+
+ 3. Neither the name of the copyright holder 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 HOLDER 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.
+ *******************************************************************************/
+
+#pragma once
+#ifndef REALSENSE_CAMERA_SYNC_NODELET_H
+#define REALSENSE_CAMERA_SYNC_NODELET_H
+
+#include <realsense_camera/base_nodelet.h>
+#include <boost/thread.hpp>
+
+namespace realsense_camera
+{
+class SyncNodelet: public realsense_camera::BaseNodelet
+{
+public:
+ // Interfaces.
+ virtual ~SyncNodelet();
+ virtual void onInit();
+
+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();
+ virtual void publishTopic(rs_stream stream_index);
+ virtual void setImageData(rs_stream stream_index);
+};
+} // namespace realsense_camera
+#endif // REALSENSE_CAMERA_SYNC_NODELET_H

realsense_camera/src/base_nodelet.cpp

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -456,7 +456,8 @@ namespace realsense_camera
 
  if (req.power_on == true)
  {
- ROS_INFO_STREAM(nodelet_name_ << " - " << startCamera());
+ start_camera_ = true;
+ start_stop_srv_called_ = true;
  }
  else
  {
@@ -468,7 +469,8 @@ namespace realsense_camera
  {
  if (checkForSubscriber() == false)
  {
- ROS_INFO_STREAM(nodelet_name_ << " - " << stopCamera());
+ start_camera_ = false;
+ start_stop_srv_called_ = true;
  }
  else
  {
@@ -478,23 +480,16 @@ namespace realsense_camera
  }
  }
  return res.success;
- }
-
+}
 
  /*
  * Force Power Camera service
  */
  bool BaseNodelet::forcePowerCameraService(realsense_camera::ForcePower::Request & req,
  realsense_camera::ForcePower::Response & res)
  {
- if (req.power_on == true)
- {
- ROS_INFO_STREAM(nodelet_name_ << " - " << startCamera());
- }
- else
- {
- ROS_INFO_STREAM(nodelet_name_ << " - " << stopCamera());
- }
+ start_camera_ = req.power_on;
+ start_stop_srv_called_ = true;
  return true;
  }
 
@@ -853,13 +848,6 @@ namespace realsense_camera
  {
  enable_[RS_STREAM_DEPTH] = true;
  }
-
- if (enable_[RS_STREAM_DEPTH] != rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0))
- {
- stopCamera();
- setStreams();
- startCamera();
- }
  }
 
  /*

realsense_camera/src/f200_nodelet.cpp

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -59,7 +59,7 @@ namespace realsense_camera
 
  max_z_ = F200_MAX_Z;
 
- BaseNodelet::onInit();
+ SyncNodelet::onInit();
  }
 
  /*
@@ -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.
  */

realsense_camera/src/r200_nodelet.cpp

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -66,7 +66,7 @@ namespace realsense_camera
 
  max_z_ = R200_MAX_Z;
 
- BaseNodelet::onInit();
+ SyncNodelet::onInit();
  }
 
  /*
@@ -417,23 +417,6 @@ namespace realsense_camera
  }
  }
 
- /*
- * Set up the callbacks for the camera streams
- */
- void R200Nodelet::setFrameCallbacks()
- {
- // call base nodelet method
- BaseNodelet::setFrameCallbacks();
-
- ir2_frame_handler_ = [&](rs::frame frame) // NOLINT(build/c++11)
- {
- publishTopic(RS_STREAM_INFRARED2, frame);
- };
-
- rs_set_frame_callback_cpp(rs_device_, RS_STREAM_INFRARED2, new rs::frame_callback(ir2_frame_handler_), &rs_error_);
- checkError();
- }
-
  /*
  * Get the camera extrinsics
  */

realsense_camera/src/sr300_nodelet.cpp

@@ -1,5 +1,5 @@
 /******************************************************************************
- Copyright (c) 2016, Intel Corporation
+ Copyright (c) 2017, Intel Corporation
  All rights reserved.
 
  Redistribution and use in source and binary forms, with or without
@@ -58,7 +58,7 @@ namespace realsense_camera
 
  max_z_ = SR300_MAX_Z;
 
- BaseNodelet::onInit();
+ SyncNodelet::onInit();
  }
 
  /*
@@ -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.
  */