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
00032
00033
00034
00035 #include "image_transport/image_transport.h"
00036 #include "image_transport/camera_common.h"
00037
00038 namespace image_transport {
00039
00040 struct CameraPublisher::Impl
00041 {
00042 Impl()
00043 : unadvertised_(false)
00044 {
00045 }
00046
00047 ~Impl()
00048 {
00049 shutdown();
00050 }
00051
00052 bool isValid() const
00053 {
00054 return !unadvertised_;
00055 }
00056
00057 void shutdown()
00058 {
00059 if (!unadvertised_) {
00060 unadvertised_ = true;
00061 image_pub_.shutdown();
00062 info_pub_.shutdown();
00063 }
00064 }
00065
00066 Publisher image_pub_;
00067 ros::Publisher info_pub_;
00068 bool unadvertised_;
00069
00070 };
00071
00072 CameraPublisher::CameraPublisher(ImageTransport& image_it, ros::NodeHandle& info_nh,
00073 const std::string& base_topic, uint32_t queue_size,
00074 const SubscriberStatusCallback& image_connect_cb,
00075 const SubscriberStatusCallback& image_disconnect_cb,
00076 const ros::SubscriberStatusCallback& info_connect_cb,
00077 const ros::SubscriberStatusCallback& info_disconnect_cb,
00078 const ros::VoidPtr& tracked_object, bool latch)
00079 : impl_(new Impl)
00080 {
00081
00082
00083 std::string image_topic = info_nh.resolveName(base_topic);
00084 std::string info_topic = getCameraInfoTopic(image_topic);
00085
00086 impl_->image_pub_ = image_it.advertise(image_topic, queue_size, image_connect_cb,
00087 image_disconnect_cb, tracked_object, latch);
00088 impl_->info_pub_ = info_nh.advertise<sensor_msgs::CameraInfo>(info_topic, queue_size, info_connect_cb,
00089 info_disconnect_cb, tracked_object, latch);
00090 }
00091
00092 uint32_t CameraPublisher::getNumSubscribers() const
00093 {
00094 if (impl_ && impl_->isValid())
00095 return std::max(impl_->image_pub_.getNumSubscribers(), impl_->info_pub_.getNumSubscribers());
00096 return 0;
00097 }
00098
00099 std::string CameraPublisher::getTopic() const
00100 {
00101 if (impl_) return impl_->image_pub_.getTopic();
00102 return std::string();
00103 }
00104
00105 std::string CameraPublisher::getInfoTopic() const
00106 {
00107 if (impl_) return impl_->info_pub_.getTopic();
00108 return std::string();
00109 }
00110
00111 void CameraPublisher::publish(const sensor_msgs::Image& image, const sensor_msgs::CameraInfo& info) const
00112 {
00113 if (!impl_ || !impl_->isValid()) {
00114 ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
00115 return;
00116 }
00117
00118 impl_->image_pub_.publish(image);
00119 impl_->info_pub_.publish(info);
00120 }
00121
00122 void CameraPublisher::publish(const sensor_msgs::ImageConstPtr& image,
00123 const sensor_msgs::CameraInfoConstPtr& info) const
00124 {
00125 if (!impl_ || !impl_->isValid()) {
00126 ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
00127 return;
00128 }
00129
00130 impl_->image_pub_.publish(image);
00131 impl_->info_pub_.publish(info);
00132 }
00133
00134 void CameraPublisher::publish(sensor_msgs::Image& image, sensor_msgs::CameraInfo& info,
00135 ros::Time stamp) const
00136 {
00137 if (!impl_ || !impl_->isValid()) {
00138 ROS_ASSERT_MSG(false, "Call to publish() on an invalid image_transport::CameraPublisher");
00139 return;
00140 }
00141
00142 image.header.stamp = stamp;
00143 info.header.stamp = stamp;
00144 publish(image, info);
00145 }
00146
00147 void CameraPublisher::shutdown()
00148 {
00149 if (impl_) {
00150 impl_->shutdown();
00151 impl_.reset();
00152 }
00153 }
00154
00155 CameraPublisher::operator void*() const
00156 {
00157 return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00158 }
00159
00160 }