data_odom_sync.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "ros/ros.h"
00029 #include "pluginlib/class_list_macros.h"
00030 #include "nodelet/nodelet.h"
00031 
00032 #include <message_filters/subscriber.h>
00033 #include <message_filters/time_synchronizer.h>
00034 #include <message_filters/sync_policies/approximate_time.h>
00035 
00036 #include <image_transport/image_transport.h>
00037 #include <image_transport/subscriber_filter.h>
00038 
00039 #include <sensor_msgs/CameraInfo.h>
00040 #include <nav_msgs/Odometry.h>
00041 
00042 namespace rtabmap_ros
00043 {
00044 
00045 class DataOdomSyncNodelet : public nodelet::Nodelet
00046 {
00047 public:
00048         //Constructor
00049         DataOdomSyncNodelet():
00050                 sync_(0)
00051         {
00052         }
00053 
00054         virtual ~DataOdomSyncNodelet()
00055         {
00056                 delete sync_;
00057         }
00058 
00059 private:
00060         virtual void onInit()
00061         {
00062                 ros::NodeHandle& nh = getNodeHandle();
00063                 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00064 
00065                 ros::NodeHandle rgb_nh(nh, "rgb");
00066                 ros::NodeHandle depth_nh(nh, "depth");
00067                 ros::NodeHandle rgb_pnh(private_nh, "rgb");
00068                 ros::NodeHandle depth_pnh(private_nh, "depth");
00069                 image_transport::ImageTransport rgb_it(rgb_nh);
00070                 image_transport::ImageTransport depth_it(depth_nh);
00071                 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00072                 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00073 
00074                 int queueSize = 10;
00075                 private_nh.param("queue_size", queueSize, queueSize);
00076 
00077                 sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_, odom_sub_);
00078                 sync_->registerCallback(boost::bind(&DataOdomSyncNodelet::callback, this, _1, _2, _3, _4));
00079 
00080                 image_sub_.subscribe(rgb_it, rgb_nh.resolveName("image_in"), 1, hintsRgb);
00081                 image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image_in"), 1, hintsDepth);
00082                 info_sub_.subscribe(rgb_nh, "camera_info_in", 1);
00083                 odom_sub_.subscribe(nh, "odom_in", 1);
00084 
00085                 imagePub_ = rgb_it.advertise("image_out", 10);
00086                 imageDepthPub_ = depth_it.advertise("image_out", 10);
00087                 infoPub_ = rgb_nh.advertise<sensor_msgs::CameraInfo>("camera_info_out", 10);
00088                 odomPub_ = nh.advertise<nav_msgs::Odometry>("odom_out", 10);
00089         };
00090 
00091         void callback(const sensor_msgs::ImageConstPtr& image,
00092                         const sensor_msgs::ImageConstPtr& imageDepth,
00093                         const sensor_msgs::CameraInfoConstPtr& camInfo,
00094                         const nav_msgs::OdometryConstPtr & odom)
00095         {
00096                 if(imagePub_.getNumSubscribers())
00097                 {
00098                         imagePub_.publish(image);
00099                 }
00100                 if(imageDepthPub_.getNumSubscribers())
00101                 {
00102                         imageDepthPub_.publish(imageDepth);
00103                 }
00104                 if(infoPub_.getNumSubscribers())
00105                 {
00106                         infoPub_.publish(camInfo);
00107                 }
00108                 if(odomPub_.getNumSubscribers())
00109                 {
00110                         odomPub_.publish(odom);
00111                 }
00112         }
00113 
00114         image_transport::Publisher imagePub_;
00115         image_transport::Publisher imageDepthPub_;
00116         ros::Publisher infoPub_;
00117         ros::Publisher odomPub_;
00118 
00119         image_transport::SubscriberFilter image_sub_;
00120         image_transport::SubscriberFilter image_depth_sub_;
00121         message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00122         message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
00123         typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry> MySyncPolicy;
00124         message_filters::Synchronizer<MySyncPolicy> * sync_;
00125 
00126 };
00127 
00128 
00129 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataOdomSyncNodelet, nodelet::Nodelet);
00130 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:07