sync_nodelet.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  Copyright (c) 2017, Intel Corporation
00003  All rights reserved.
00004 
00005  Redistribution and use in source and binary forms, with or without
00006  modification, are permitted provided that the following conditions are met:
00007 
00008  1. Redistributions of source code must retain the above copyright notice, this
00009  list of conditions and the following disclaimer.
00010 
00011  2. Redistributions in binary form must reproduce the above copyright notice,
00012  this list of conditions and the following disclaimer in the documentation
00013  and/or other materials provided with the distribution.
00014 
00015  3. Neither the name of the copyright holder nor the names of its contributors
00016  may be used to endorse or promote products derived from this software without
00017  specific prior written permission.
00018 
00019  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00023  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00024  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00025  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00027  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00028  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  *******************************************************************************/
00030 
00031 #include <realsense_camera/sync_nodelet.h>
00032 
00033 PLUGINLIB_EXPORT_CLASS(realsense_camera::SyncNodelet, nodelet::Nodelet)
00034 
00035 namespace realsense_camera
00036 {
00037   SyncNodelet::~SyncNodelet()
00038   {
00039     topic_thread_->join();
00040   }
00041 
00042   /*
00043    * Initialize the nodelet.
00044    */
00045   void SyncNodelet::onInit()
00046   {
00047     BaseNodelet::onInit();
00048 
00049     // start thread to publish topics
00050     topic_thread_ =
00051             boost::shared_ptr <boost::thread>(new boost::thread (boost::bind(&SyncNodelet::publishSyncTopics, this)));
00052   }
00053 
00054   void SyncNodelet::publishSyncTopics() try
00055   {
00056     while (ros::ok())
00057     {
00058       if (start_stop_srv_called_ == true)
00059       {
00060         if (start_camera_ == true)
00061         {
00062           ROS_INFO_STREAM(nodelet_name_ << " - " << startCamera());
00063         }
00064         else
00065         {
00066           ROS_INFO_STREAM(nodelet_name_ << " - " << stopCamera());
00067         }
00068 
00069         start_stop_srv_called_ = false;
00070       }
00071 
00072       if (enable_[RS_STREAM_DEPTH] != rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0))
00073       {
00074         stopCamera();
00075         setStreams();
00076         startCamera();
00077       }
00078 
00079       if (rs_is_device_streaming(rs_device_, 0) == 1)
00080       {
00081         rs_wait_for_frames(rs_device_, &rs_error_);
00082         checkError();
00083         topic_ts_ = ros::Time::now();
00084         duplicate_depth_color_ = false;
00085 
00086         for (int stream=0; stream < STREAM_COUNT; stream++)
00087         {
00088           if (enable_[stream] == true)
00089           {
00090             publishTopic(static_cast<rs_stream>(stream));
00091           }
00092         }
00093 
00094         if (pointcloud_publisher_.getNumSubscribers() > 0 &&
00095             rs_is_stream_enabled(rs_device_, RS_STREAM_DEPTH, 0) == 1 && enable_pointcloud_ == true &&
00096             (duplicate_depth_color_ == false))  // Skip publishing PointCloud if Depth or Color frame was duplicate
00097         {
00098           if (camera_publisher_[RS_STREAM_DEPTH].getNumSubscribers() <= 0)
00099           {
00100             setImageData(RS_STREAM_DEPTH);
00101           }
00102 
00103           if (camera_publisher_[RS_STREAM_COLOR].getNumSubscribers() <= 0)
00104           {
00105             setImageData(RS_STREAM_COLOR);
00106           }
00107 
00108           publishPCTopic();
00109         }
00110       }
00111     }
00112   }
00113   catch(const rs::error & e)
00114   {
00115     ROS_ERROR_STREAM(nodelet_name_ << " - " << "RealSense error calling "
00116         << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    "
00117         << e.what());
00118     ros::shutdown();
00119   }
00120   catch(const std::exception & e)
00121   {
00122     ROS_ERROR_STREAM(nodelet_name_ << " - " << e.what());
00123     ros::shutdown();
00124   }
00125   catch(...)
00126   {
00127     ROS_ERROR_STREAM(nodelet_name_ << " - Caught unknown expection...shutting down!");
00128     ros::shutdown();
00129   }
00130 
00131   /*
00132    * Publish topic.
00133    */
00134   void SyncNodelet::publishTopic(rs_stream stream_index)
00135   {
00136     // Publish stream only if there is at least one subscriber.
00137     if (camera_publisher_[stream_index].getNumSubscribers() > 0 &&
00138         rs_is_stream_enabled(rs_device_, (rs_stream) stream_index, 0) == 1)
00139     {
00140       double frame_ts = rs_get_frame_timestamp(rs_device_, (rs_stream) stream_index, 0);
00141       if (ts_[stream_index] != frame_ts)  // Publish frames only if its not duplicate
00142       {
00143         setImageData(stream_index);
00144 
00145         sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(),
00146             encoding_[stream_index],
00147             image_[stream_index]).toImageMsg();
00148 
00149         msg->header.frame_id = optical_frame_id_[stream_index];
00150         msg->header.stamp = topic_ts_;  // Publish timestamp to synchronize frames.
00151         msg->width = image_[stream_index].cols;
00152         msg->height = image_[stream_index].rows;
00153         msg->is_bigendian = false;
00154         msg->step = step_[stream_index];
00155 
00156         camera_info_ptr_[stream_index]->header.stamp = msg->header.stamp;
00157         camera_publisher_[stream_index].publish(msg, camera_info_ptr_[stream_index]);
00158       }
00159       else
00160       {
00161         if ((stream_index == RS_STREAM_DEPTH) || (stream_index == RS_STREAM_COLOR))
00162         {
00163           duplicate_depth_color_ = true;  // Set this flag to true if Depth and/or Color frame is duplicate
00164         }
00165       }
00166 
00167       ts_[stream_index] = frame_ts;
00168     }
00169   }
00170 
00171   void SyncNodelet::setImageData(rs_stream stream_index)
00172     {
00173       if (stream_index == RS_STREAM_DEPTH)
00174       {
00175         // fill depth buffer
00176         image_depth16_ = reinterpret_cast<const uint16_t *>(rs_get_frame_data(rs_device_, stream_index, 0));
00177         float depth_scale_meters = rs_get_device_depth_scale(rs_device_, &rs_error_);
00178         if (depth_scale_meters == MILLIMETER_METERS)
00179         {
00180           image_[stream_index].data = (unsigned char *) image_depth16_;
00181         }
00182         else
00183         {
00184           cvWrapper_ = cv::Mat(image_[stream_index].size(), cv_type_[stream_index],
00185                                const_cast<void *>(reinterpret_cast<const void *>(image_depth16_)),
00186                                step_[stream_index]);
00187           cvWrapper_.convertTo(image_[stream_index], cv_type_[stream_index],
00188                 static_cast<double>(depth_scale_meters) / static_cast<double>(MILLIMETER_METERS));
00189         }
00190       }
00191       else if (stream_index == RS_STREAM_COLOR)
00192       {
00193         image_[stream_index].data = (unsigned char *) (rs_get_frame_data(rs_device_, RS_STREAM_RECTIFIED_COLOR, 0));
00194       }
00195       else
00196       {
00197         image_[stream_index].data = (unsigned char *) (rs_get_frame_data(rs_device_, stream_index, 0));
00198       }
00199     }
00200 }  // namespace realsense_camera


realsense_camera
Author(s): Rajvi Jingar , Reagan Lopez , Matt Hansen , Mark Horn
autogenerated on Thu Jun 6 2019 21:15:58