Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
00044
00045 void SyncNodelet::onInit()
00046 {
00047 BaseNodelet::onInit();
00048
00049
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))
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
00133
00134 void SyncNodelet::publishTopic(rs_stream stream_index)
00135 {
00136
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)
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_;
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;
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
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 }