00001 #include "polled_camera/publication_server.h"
00002 #include <image_transport/image_transport.h>
00003
00004 namespace polled_camera {
00005
00007
00008 class PublicationServer::Impl
00009 {
00010 public:
00011 ros::ServiceServer srv_server_;
00012 DriverCallback driver_cb_;
00013 ros::VoidPtr tracked_object_;
00014 image_transport::ImageTransport it_;
00015 std::map<std::string, image_transport::CameraPublisher> client_map_;
00016 bool unadvertised_;
00017 double constructed_;
00018
00019 Impl(const ros::NodeHandle& nh)
00020 : it_(nh),
00021 unadvertised_(false),
00022 constructed_(ros::WallTime::now().toSec())
00023 {
00024 }
00025
00026 ~Impl()
00027 {
00028 if (ros::WallTime::now().toSec() - constructed_ < 0.001)
00029 ROS_WARN("PublicationServer destroyed immediately after creation. Did you forget to store the handle?");
00030 unadvertise();
00031 }
00032
00033 bool isValid() const
00034 {
00035 return !unadvertised_;
00036 }
00037
00038 void unadvertise()
00039 {
00040 if (!unadvertised_) {
00041 unadvertised_ = true;
00042 srv_server_.shutdown();
00043 client_map_.clear();
00044 }
00045 }
00046
00047 bool requestCallback(polled_camera::GetPolledImage::Request& req,
00048 polled_camera::GetPolledImage::Response& rsp)
00049 {
00050 std::string image_topic = req.response_namespace + "/image_raw";
00051 image_transport::CameraPublisher& pub = client_map_[image_topic];
00052 if (!pub) {
00053
00054 pub = it_.advertiseCamera(image_topic, 1, image_transport::SubscriberStatusCallback(),
00055 boost::bind(&Impl::disconnectCallback, this, _1),
00056 ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(),
00057 ros::VoidPtr(), true );
00058 ROS_INFO("Advertising %s", pub.getTopic().c_str());
00059 }
00060
00061
00062 req.binning_x = std::max(req.binning_x, (uint32_t)1);
00063 req.binning_y = std::max(req.binning_y, (uint32_t)1);
00064
00066 sensor_msgs::Image image;
00067 sensor_msgs::CameraInfo info;
00068 driver_cb_(req, rsp, image, info);
00069
00070 if (rsp.success) {
00071 assert(image.header.stamp == info.header.stamp);
00072 rsp.stamp = image.header.stamp;
00073 pub.publish(image, info);
00074 }
00075 else {
00076 ROS_ERROR("Failed to capture requested image, status message: '%s'",
00077 rsp.status_message.c_str());
00078 }
00079
00080 return true;
00081 }
00082
00083 void disconnectCallback(const image_transport::SingleSubscriberPublisher& ssp)
00084 {
00085
00086 if (ssp.getNumSubscribers() == 0) {
00087 ROS_INFO("Shutting down %s", ssp.getTopic().c_str());
00088 client_map_.erase(ssp.getTopic());
00089 }
00090 }
00091 };
00092
00094
00095 PublicationServer::PublicationServer(const std::string& service, ros::NodeHandle& nh,
00096 const DriverCallback& cb, const ros::VoidPtr& tracked_object)
00097 : impl_(new Impl(nh))
00098 {
00099 impl_->driver_cb_ = cb;
00100 impl_->tracked_object_ = tracked_object;
00101 impl_->srv_server_ = nh.advertiseService<>(service, &Impl::requestCallback, impl_);
00102 }
00103
00104 void PublicationServer::shutdown()
00105 {
00106 if (impl_) impl_->unadvertise();
00107 }
00108
00109 std::string PublicationServer::getService() const
00110 {
00111 if (impl_) return impl_->srv_server_.getService();
00112 return std::string();
00113 }
00114
00115 PublicationServer::operator void*() const
00116 {
00117 return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0;
00118 }
00119
00120 PublicationServer advertise(ros::NodeHandle& nh, const std::string& service,
00121 const PublicationServer::DriverCallback& cb,
00122 const ros::VoidPtr& tracked_object)
00123 {
00124 return PublicationServer(service, nh, cb, tracked_object);
00125 }
00126
00127 }