CoreWrapper.h
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 #ifndef COREWRAPPER_H_
00029 #define COREWRAPPER_H_
00030 
00031 
00032 #include <ros/ros.h>
00033 
00034 #include <std_srvs/Empty.h>
00035 
00036 #include <tf/transform_listener.h>
00037 #include <tf2_ros/transform_broadcaster.h>
00038 
00039 #include <std_msgs/Empty.h>
00040 #include <std_msgs/Int32.h>
00041 #include <sensor_msgs/PointCloud2.h>
00042 #include <sensor_msgs/Image.h>
00043 #include <sensor_msgs/CameraInfo.h>
00044 #include <sensor_msgs/LaserScan.h>
00045 #include <nav_msgs/Odometry.h>
00046 #include <nav_msgs/GetMap.h>
00047 
00048 #include <rtabmap/core/Parameters.h>
00049 #include <rtabmap/core/Rtabmap.h>
00050 
00051 #include "rtabmap_ros/GetMap.h"
00052 #include "rtabmap_ros/ListLabels.h"
00053 #include "rtabmap_ros/PublishMap.h"
00054 #include "rtabmap_ros/SetGoal.h"
00055 #include "rtabmap_ros/SetLabel.h"
00056 #include "rtabmap_ros/Goal.h"
00057 
00058 #include "MapsManager.h"
00059 
00060 #include <message_filters/subscriber.h>
00061 #include <message_filters/synchronizer.h>
00062 #include <message_filters/sync_policies/approximate_time.h>
00063 #include <message_filters/sync_policies/exact_time.h>
00064 
00065 #include <image_transport/image_transport.h>
00066 #include <image_transport/subscriber_filter.h>
00067 
00068 #ifdef WITH_OCTOMAP_ROS
00069 #include <octomap_msgs/GetOctomap.h>
00070 #endif
00071 
00072 #include <actionlib/client/simple_action_client.h>
00073 #include <move_base_msgs/MoveBaseAction.h>
00074 #include <move_base_msgs/MoveBaseActionGoal.h>
00075 #include <move_base_msgs/MoveBaseActionResult.h>
00076 #include <move_base_msgs/MoveBaseActionFeedback.h>
00077 #include <actionlib_msgs/GoalStatusArray.h>
00078 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00079 
00080 class CoreWrapper
00081 {
00082 public:
00083         CoreWrapper(bool deleteDbOnStart, const rtabmap::ParametersMap & parameters);
00084         virtual ~CoreWrapper();
00085 
00086 private:
00087         void setupCallbacks(
00088                         bool subscribeDepth,
00089                         bool subscribeScan2d,
00090                         bool subscribeScan3d,
00091                         bool subscribeStereo,
00092                         int queueSize,
00093                         bool approxSync,
00094                         int depthCameras);
00095         void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
00096 
00097         bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
00098         bool commonOdomTFUpdate(const ros::Time & stamp); // TF odom
00099         rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
00100 
00101         void commonDepthCallback(
00102                                 const std::string & odomFrameId,
00103                                 const sensor_msgs::ImageConstPtr& imageMsg,
00104                                 const sensor_msgs::ImageConstPtr& depthMsg,
00105                                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00106                                 const sensor_msgs::LaserScanConstPtr& scanMsg,
00107                                 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
00108         void commonDepthCallback(
00109                                 const std::string & odomFrameId,
00110                                 const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
00111                                 const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
00112                                 const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
00113                                 const sensor_msgs::LaserScanConstPtr& scanMsg,
00114                                 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
00115         void commonStereoCallback(
00116                                 const std::string & odomFrameId,
00117                                 const sensor_msgs::ImageConstPtr& leftImageMsg,
00118                                 const sensor_msgs::ImageConstPtr& rightImageMsg,
00119                                 const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00120                                 const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
00121                                 const sensor_msgs::LaserScanConstPtr& scanMsg,
00122                                 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg);
00123 
00124         // with odom msg
00125         void depthCallback(
00126                         const sensor_msgs::ImageConstPtr& imageMsg,
00127                         const nav_msgs::OdometryConstPtr & odomMsg,
00128                         const sensor_msgs::ImageConstPtr& imageDepthMsg,
00129                         const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
00130         void depthScanCallback(
00131                         const sensor_msgs::ImageConstPtr& imageMsg,
00132                         const nav_msgs::OdometryConstPtr & odomMsg,
00133                         const sensor_msgs::ImageConstPtr& imageDepthMsg,
00134                         const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
00135                         const sensor_msgs::LaserScanConstPtr& scanMsg);
00136         void depthScan3dCallback(
00137                         const sensor_msgs::ImageConstPtr& imageMsg,
00138                         const nav_msgs::OdometryConstPtr & odomMsg,
00139                         const sensor_msgs::ImageConstPtr& imageDepthMsg,
00140                         const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
00141                         const sensor_msgs::PointCloud2ConstPtr& scanMsg);
00142         void stereoCallback(
00143                         const sensor_msgs::ImageConstPtr& leftImageMsg,
00144                         const sensor_msgs::ImageConstPtr& rightImageMsg,
00145                         const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00146                         const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
00147                         const nav_msgs::OdometryConstPtr & odomMsg);
00148         void stereoScanCallback(
00149                         const sensor_msgs::ImageConstPtr& leftImageMsg,
00150                         const sensor_msgs::ImageConstPtr& rightImageMsg,
00151                         const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00152                         const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
00153                         const sensor_msgs::LaserScanConstPtr& scanMsg,
00154                         const nav_msgs::OdometryConstPtr & odomMsg);
00155         void stereoScan3dCallback(
00156                         const sensor_msgs::ImageConstPtr& leftImageMsg,
00157                         const sensor_msgs::ImageConstPtr& rightImageMsg,
00158                         const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00159                         const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
00160                         const sensor_msgs::PointCloud2ConstPtr& scanMsg,
00161                         const nav_msgs::OdometryConstPtr & odomMsg);
00162         void depth2Callback(
00163                         const nav_msgs::OdometryConstPtr & odomMsg,
00164                         const sensor_msgs::ImageConstPtr& image1Msg,
00165                         const sensor_msgs::ImageConstPtr& imageDepth1Msg,
00166                         const sensor_msgs::CameraInfoConstPtr& camInfo1Msg,
00167                         const sensor_msgs::ImageConstPtr& image2Msg,
00168                         const sensor_msgs::ImageConstPtr& imageDept2hMsg,
00169                         const sensor_msgs::CameraInfoConstPtr& camInfo2Msg);
00170 
00171         // without odom, when TF is used for odom
00172         void depthTFCallback(
00173                         const sensor_msgs::ImageConstPtr& imageMsg,
00174                         const sensor_msgs::ImageConstPtr& imageDepthMsg,
00175                         const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
00176         void depthScanTFCallback(
00177                         const sensor_msgs::ImageConstPtr& imageMsg,
00178                         const sensor_msgs::ImageConstPtr& imageDepthMsg,
00179                         const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
00180                         const sensor_msgs::LaserScanConstPtr& scanMsg);
00181         void depthScan3dTFCallback(
00182                         const sensor_msgs::ImageConstPtr& imageMsg,
00183                         const sensor_msgs::ImageConstPtr& imageDepthMsg,
00184                         const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
00185                         const sensor_msgs::PointCloud2ConstPtr& scanMsg);
00186         void stereoTFCallback(
00187                         const sensor_msgs::ImageConstPtr& leftImageMsg,
00188                         const sensor_msgs::ImageConstPtr& rightImageMsg,
00189                         const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00190                         const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg);
00191         void stereoScanTFCallback(
00192                         const sensor_msgs::ImageConstPtr& leftImageMsg,
00193                         const sensor_msgs::ImageConstPtr& rightImageMsg,
00194                         const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00195                         const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
00196                         const sensor_msgs::LaserScanConstPtr& scanMsg);
00197         void stereoScan3dTFCallback(
00198                         const sensor_msgs::ImageConstPtr& leftImageMsg,
00199                         const sensor_msgs::ImageConstPtr& rightImageMsg,
00200                         const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
00201                         const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
00202                         const sensor_msgs::PointCloud2ConstPtr& scanMsg);
00203 
00204         void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
00205         void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
00206         void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
00207         void updateGoal(const ros::Time & stamp);
00208 
00209         void process(
00210                         const ros::Time & stamp,
00211                         const rtabmap::SensorData & data,
00212                         const rtabmap::Transform & odom = rtabmap::Transform(),
00213                         const std::string & odomFrameId = "",
00214                         float odomRotationalVariance = 1.0,
00215                         float odomTransitionalVariance = 1.0);
00216 
00217         bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00218         bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00219         bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00220         bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00221         bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00222         bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00223         bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00224         bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00225         bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00226         bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00227         bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00228         bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00229         bool getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
00230         bool getProjMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
00231         bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
00232         bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
00233         bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
00234         bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00235         bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
00236         bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
00237 #ifdef WITH_OCTOMAP_ROS
00238         bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
00239         bool octomapFullCallback(octomap_msgs::GetOctomap::Request  &req, octomap_msgs::GetOctomap::Response &res);
00240 #endif
00241 
00242         rtabmap::ParametersMap loadParameters(const std::string & configFile);
00243         void saveParameters(const std::string & configFile);
00244 
00245         void publishLoop(double tfDelay, double tfTolerance);
00246 
00247         void publishStats(const ros::Time & stamp);
00248         void publishCurrentGoal(const ros::Time & stamp);
00249         void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
00250         void goalActiveCb();
00251         void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
00252         void publishLocalPath(const ros::Time & stamp);
00253         void publishGlobalPath(const ros::Time & stamp);
00254 
00255 private:
00256         rtabmap::Rtabmap rtabmap_;
00257         bool paused_;
00258         rtabmap::Transform lastPose_;
00259         ros::Time lastPoseStamp_;
00260         bool lastPoseIntermediate_;
00261         float rotVariance_;
00262         float transVariance_;
00263         rtabmap::Transform currentMetricGoal_;
00264         bool latestNodeWasReached_;
00265         rtabmap::ParametersMap parameters_;
00266 
00267         std::string frameId_;
00268         std::string mapFrameId_;
00269         std::string odomFrameId_;
00270         std::string groundTruthFrameId_;
00271         std::string configPath_;
00272         std::string databasePath_;
00273         bool waitForTransform_;
00274         double waitForTransformDuration_;
00275         bool useActionForGoal_;
00276         bool genScan_;
00277         double genScanMaxDepth_;
00278         double genScanMinDepth_;
00279         int scanCloudMaxPoints_;
00280         int scanCloudNormalK_;
00281         bool flipScan_;
00282 
00283         rtabmap::Transform mapToOdom_;
00284         boost::mutex mapToOdomMutex_;
00285 
00286         MapsManager mapsManager_;
00287 
00288         ros::Publisher infoPub_;
00289         ros::Publisher mapDataPub_;
00290         ros::Publisher mapGraphPub_;
00291         ros::Publisher labelsPub_;
00292 
00293         //Planning stuff
00294         ros::Subscriber goalSub_;
00295         ros::Subscriber goalNodeSub_;
00296         ros::Publisher nextMetricGoalPub_;
00297         ros::Publisher goalReachedPub_;
00298         ros::Publisher globalPathPub_;
00299         ros::Publisher localPathPub_;
00300 
00301         // for loop closure detection only
00302         image_transport::Subscriber defaultSub_;
00303 
00304         //for depth callback
00305         std::vector<image_transport::SubscriberFilter*> imageSubs_;
00306         std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
00307         std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;
00308 
00309         //stereo callback
00310         image_transport::SubscriberFilter imageRectLeft_;
00311         image_transport::SubscriberFilter imageRectRight_;
00312         message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00313         message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00314 
00315         message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
00316         message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
00317         message_filters::Subscriber<sensor_msgs::PointCloud2> scan3dSub_;
00318 
00319         typedef message_filters::sync_policies::ApproximateTime<
00320                         sensor_msgs::Image,
00321                         nav_msgs::Odometry,
00322                         sensor_msgs::Image,
00323                         sensor_msgs::CameraInfo,
00324                         sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
00325         message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
00326 
00327         typedef message_filters::sync_policies::ApproximateTime<
00328                         sensor_msgs::Image,
00329                         nav_msgs::Odometry,
00330                         sensor_msgs::Image,
00331                         sensor_msgs::CameraInfo,
00332                         sensor_msgs::PointCloud2> MyDepthScan3dSyncPolicy;
00333         message_filters::Synchronizer<MyDepthScan3dSyncPolicy> * depthScan3dSync_;
00334 
00335         typedef message_filters::sync_policies::ApproximateTime<
00336                         sensor_msgs::Image,
00337                         nav_msgs::Odometry,
00338                         sensor_msgs::Image,
00339                         sensor_msgs::CameraInfo> MyDepthSyncPolicy;
00340         message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
00341         typedef message_filters::sync_policies::ExactTime<
00342                         sensor_msgs::Image,
00343                         nav_msgs::Odometry,
00344                         sensor_msgs::Image,
00345                         sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
00346         message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;
00347 
00348         typedef message_filters::sync_policies::ApproximateTime<
00349                         sensor_msgs::Image,
00350                         sensor_msgs::Image,
00351                         sensor_msgs::CameraInfo,
00352                         sensor_msgs::CameraInfo,
00353                         sensor_msgs::LaserScan,
00354                         nav_msgs::Odometry> MyStereoScanSyncPolicy;
00355         message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
00356 
00357         typedef message_filters::sync_policies::ApproximateTime<
00358                         sensor_msgs::Image,
00359                         sensor_msgs::Image,
00360                         sensor_msgs::CameraInfo,
00361                         sensor_msgs::CameraInfo,
00362                         sensor_msgs::PointCloud2,
00363                         nav_msgs::Odometry> MyStereoScan3dSyncPolicy;
00364         message_filters::Synchronizer<MyStereoScan3dSyncPolicy> * stereoScan3dSync_;
00365 
00366         typedef message_filters::sync_policies::ApproximateTime<
00367                         sensor_msgs::Image,
00368                         sensor_msgs::Image,
00369                         sensor_msgs::CameraInfo,
00370                         sensor_msgs::CameraInfo,
00371                         nav_msgs::Odometry> MyStereoApproxSyncPolicy;
00372         message_filters::Synchronizer<MyStereoApproxSyncPolicy> * stereoApproxSync_;
00373 
00374         typedef message_filters::sync_policies::ExactTime<
00375                         sensor_msgs::Image,
00376                         sensor_msgs::Image,
00377                         sensor_msgs::CameraInfo,
00378                         sensor_msgs::CameraInfo,
00379                         nav_msgs::Odometry> MyStereoExactSyncPolicy;
00380         message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;
00381 
00382         typedef message_filters::sync_policies::ApproximateTime<
00383                         nav_msgs::Odometry,
00384                         sensor_msgs::Image,
00385                         sensor_msgs::Image,
00386                         sensor_msgs::CameraInfo,
00387                         sensor_msgs::Image,
00388                         sensor_msgs::Image,
00389                         sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
00390         message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
00391 
00392         // without odom, when TF is used for odom
00393         typedef message_filters::sync_policies::ApproximateTime<
00394                         sensor_msgs::Image,
00395                         sensor_msgs::Image,
00396                         sensor_msgs::CameraInfo,
00397                         sensor_msgs::LaserScan> MyDepthScanTFSyncPolicy;
00398         message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
00399 
00400         typedef message_filters::sync_policies::ApproximateTime<
00401                         sensor_msgs::Image,
00402                         sensor_msgs::Image,
00403                         sensor_msgs::CameraInfo,
00404                         sensor_msgs::PointCloud2> MyDepthScan3dTFSyncPolicy;
00405         message_filters::Synchronizer<MyDepthScan3dTFSyncPolicy> * depthScan3dTFSync_;
00406 
00407         typedef message_filters::sync_policies::ApproximateTime<
00408                         sensor_msgs::Image,
00409                         sensor_msgs::Image,
00410                         sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
00411         message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
00412         typedef message_filters::sync_policies::ExactTime<
00413                                 sensor_msgs::Image,
00414                                 sensor_msgs::Image,
00415                                 sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
00416         message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;
00417 
00418         typedef message_filters::sync_policies::ApproximateTime<
00419                         sensor_msgs::Image,
00420                         sensor_msgs::Image,
00421                         sensor_msgs::CameraInfo,
00422                         sensor_msgs::CameraInfo,
00423                         sensor_msgs::LaserScan> MyStereoScanTFSyncPolicy;
00424         message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
00425 
00426         typedef message_filters::sync_policies::ApproximateTime<
00427                         sensor_msgs::Image,
00428                         sensor_msgs::Image,
00429                         sensor_msgs::CameraInfo,
00430                         sensor_msgs::CameraInfo,
00431                         sensor_msgs::PointCloud2> MyStereoScan3dTFSyncPolicy;
00432         message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy> * stereoScan3dTFSync_;
00433 
00434         typedef message_filters::sync_policies::ApproximateTime<
00435                         sensor_msgs::Image,
00436                         sensor_msgs::Image,
00437                         sensor_msgs::CameraInfo,
00438                         sensor_msgs::CameraInfo> MyStereoApproxTFSyncPolicy;
00439         message_filters::Synchronizer<MyStereoApproxTFSyncPolicy> * stereoApproxTFSync_;
00440 
00441         typedef message_filters::sync_policies::ExactTime<
00442                         sensor_msgs::Image,
00443                         sensor_msgs::Image,
00444                         sensor_msgs::CameraInfo,
00445                         sensor_msgs::CameraInfo> MyStereoExactTFSyncPolicy;
00446         message_filters::Synchronizer<MyStereoExactTFSyncPolicy> * stereoExactTFSync_;
00447 
00448         tf2_ros::TransformBroadcaster tfBroadcaster_;
00449         tf::TransformListener tfListener_;
00450 
00451         ros::ServiceServer updateSrv_;
00452         ros::ServiceServer resetSrv_;
00453         ros::ServiceServer pauseSrv_;
00454         ros::ServiceServer resumeSrv_;
00455         ros::ServiceServer triggerNewMapSrv_;
00456         ros::ServiceServer backupDatabase_;
00457         ros::ServiceServer setModeLocalizationSrv_;
00458         ros::ServiceServer setModeMappingSrv_;
00459         ros::ServiceServer setLogDebugSrv_;
00460         ros::ServiceServer setLogInfoSrv_;
00461         ros::ServiceServer setLogWarnSrv_;
00462         ros::ServiceServer setLogErrorSrv_;
00463         ros::ServiceServer getMapDataSrv_;
00464         ros::ServiceServer getProjMapSrv_;
00465         ros::ServiceServer getGridMapSrv_;
00466         ros::ServiceServer publishMapDataSrv_;
00467         ros::ServiceServer setGoalSrv_;
00468         ros::ServiceServer cancelGoalSrv_;
00469         ros::ServiceServer setLabelSrv_;
00470         ros::ServiceServer listLabelsSrv_;
00471 #ifdef WITH_OCTOMAP_ROS
00472         ros::ServiceServer octomapBinarySrv_;
00473         ros::ServiceServer octomapFullSrv_;
00474 #endif
00475 
00476         MoveBaseClient mbClient_;
00477 
00478         boost::thread* transformThread_;
00479 
00480         float rate_;
00481         bool createIntermediateNodes_;
00482         ros::Time time_;
00483         ros::Time previousStamp_;
00484 };
00485 
00486 #endif /* COREWRAPPER_H_ */
00487

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