CoreWrapper.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef COREWRAPPER_H_
29 #define COREWRAPPER_H_
30 
31 
32 #include <ros/ros.h>
33 #include <nodelet/nodelet.h>
34 
35 #include <std_srvs/Empty.h>
36 
37 #include <tf/transform_listener.h>
39 
40 #include <std_msgs/Empty.h>
41 #include <std_msgs/Int32.h>
42 #include <sensor_msgs/NavSatFix.h>
43 #include <nav_msgs/GetMap.h>
44 #include <nav_msgs/GetPlan.h>
45 #include <geometry_msgs/PoseWithCovarianceStamped.h>
46 #include <sensor_msgs/Imu.h>
47 
49 #include <rtabmap/core/Rtabmap.h>
51 
52 #include "rtabmap_ros/GetNodeData.h"
53 #include "rtabmap_ros/GetMap.h"
54 #include "rtabmap_ros/GetMap2.h"
55 #include "rtabmap_ros/ListLabels.h"
56 #include "rtabmap_ros/PublishMap.h"
57 #include "rtabmap_ros/SetGoal.h"
58 #include "rtabmap_ros/SetLabel.h"
59 #include "rtabmap_ros/Goal.h"
60 #include "rtabmap_ros/GetPlan.h"
62 #include "rtabmap_ros/OdomInfo.h"
63 #include "rtabmap_ros/AddLink.h"
64 #include "rtabmap_ros/GetNodesInRadius.h"
65 
66 #include "MapsManager.h"
67 
68 #ifdef WITH_OCTOMAP_MSGS
69 #include <octomap_msgs/GetOctomap.h>
70 #endif
71 
72 #ifdef WITH_APRILTAG_ROS
73 #include <apriltag_ros/AprilTagDetectionArray.h>
74 #endif
75 
77 #include <move_base_msgs/MoveBaseAction.h>
78 #include <move_base_msgs/MoveBaseActionGoal.h>
79 #include <move_base_msgs/MoveBaseActionResult.h>
80 #include <move_base_msgs/MoveBaseActionFeedback.h>
81 #include <actionlib_msgs/GoalStatusArray.h>
83 
84 namespace rtabmap {
85 class StereoDense;
86 }
87 
88 namespace rtabmap_ros {
89 
91 {
92 public:
93  CoreWrapper();
94  virtual ~CoreWrapper();
95 
96 private:
97 
98  virtual void onInit();
99 
100  bool odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Time stamp);
101  bool odomTFUpdate(const ros::Time & stamp); // TF odom
102 
103  virtual void commonDepthCallback(
104  const nav_msgs::OdometryConstPtr & odomMsg,
105  const rtabmap_ros::UserDataConstPtr & userDataMsg,
106  const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
107  const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
108  const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
109  const sensor_msgs::LaserScan& scanMsg,
110  const sensor_msgs::PointCloud2& scan3dMsg,
111  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
112  const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
113  const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
114  const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
115  const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
116  void commonDepthCallbackImpl(
117  const std::string & odomFrameId,
118  const rtabmap_ros::UserDataConstPtr & userDataMsg,
119  const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
120  const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
121  const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
122  const sensor_msgs::LaserScan& scan2dMsg,
123  const sensor_msgs::PointCloud2& scan3dMsg,
124  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
125  const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs,
126  const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints,
127  const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d,
128  const std::vector<cv::Mat> & localDescriptors);
129  virtual void commonStereoCallback(
130  const nav_msgs::OdometryConstPtr & odomMsg,
131  const rtabmap_ros::UserDataConstPtr & userDataMsg,
132  const cv_bridge::CvImageConstPtr& leftImageMsg,
133  const cv_bridge::CvImageConstPtr& rightImageMsg,
134  const sensor_msgs::CameraInfo& leftCamInfoMsg,
135  const sensor_msgs::CameraInfo& rightCamInfoMsg,
136  const sensor_msgs::LaserScan& scanMsg,
137  const sensor_msgs::PointCloud2& scan3dMsg,
138  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
139  const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
140  const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
141  const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
142  const cv::Mat & localDescriptors = cv::Mat());
143  virtual void commonLaserScanCallback(
144  const nav_msgs::OdometryConstPtr & odomMsg,
145  const rtabmap_ros::UserDataConstPtr & userDataMsg,
146  const sensor_msgs::LaserScan& scanMsg,
147  const sensor_msgs::PointCloud2& scan3dMsg,
148  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
149  const rtabmap_ros::GlobalDescriptor & globalDescriptor = rtabmap_ros::GlobalDescriptor());
150  virtual void commonOdomCallback(
151  const nav_msgs::OdometryConstPtr & odomMsg,
152  const rtabmap_ros::UserDataConstPtr & userDataMsg,
153  const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
154 
155  void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
156 
157  void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
158  void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg);
159  void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg);
160 #ifdef WITH_APRILTAG_ROS
161  void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
162 #endif
163  void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
164  void interOdomCallback(const nav_msgs::OdometryConstPtr & msg);
165  void interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2);
166 
167  void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
168 
169  void goalCommonCallback(int id,
170  const std::string & label,
171  const std::string & frameId,
172  const rtabmap::Transform & pose,
173  const ros::Time & stamp,
174  double * planningTime = 0);
175  void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
176  void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
177  void updateGoal(const ros::Time & stamp);
178 
179  void process(
180  const ros::Time & stamp,
181  rtabmap::SensorData & data,
182  const rtabmap::Transform & odom = rtabmap::Transform(),
183  const std::string & odomFrameId = "",
184  const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
185  const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo());
186 
187  bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
188  bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
189  bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
190  bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
191  bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
192  bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
193  bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
194  bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
195  bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
196  bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
197  bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
198  bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
199  bool getNodeDataCallback(rtabmap_ros::GetNodeData::Request& req, rtabmap_ros::GetNodeData::Response& res);
200  bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
201  bool getMapData2Callback(rtabmap_ros::GetMap2::Request& req, rtabmap_ros::GetMap2::Response& res);
202  bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
203  bool getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
204  bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
205  bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
206  bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
207  bool getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &res);
208  bool getPlanNodesCallback(rtabmap_ros::GetPlan::Request &req, rtabmap_ros::GetPlan::Response &res);
209  bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
210  bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
211  bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
212  bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
213  bool addLinkCallback(rtabmap_ros::AddLink::Request&, rtabmap_ros::AddLink::Response&);
214  bool getNodesInRadiusCallback(rtabmap_ros::GetNodesInRadius::Request&, rtabmap_ros::GetNodesInRadius::Response&);
215 #ifdef WITH_OCTOMAP_MSGS
216  bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
217  bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
218 #endif
219 
220  void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters);
221  void saveParameters(const std::string & configFile);
222 
223  void publishLoop(double tfDelay, double tfTolerance);
224 
225  void publishStats(const ros::Time & stamp);
226  void publishCurrentGoal(const ros::Time & stamp);
227  void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
228  void goalActiveCb();
229  void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
230  void publishLocalPath(const ros::Time & stamp);
231  void publishGlobalPath(const ros::Time & stamp);
232 
233 private:
235  bool paused_;
239  cv::Mat covariance_;
244  std::map<std::string, float> rtabmapROSStats_;
245 
246  std::string frameId_;
247  std::string odomFrameId_;
248  std::string mapFrameId_;
249  std::string groundTruthFrameId_;
251  std::string configPath_;
252  std::string databasePath_;
261  bool genScan_;
264  bool genDepth_;
270 
272  boost::mutex mapToOdomMutex_;
273 
275 
287 
288  //Planning stuff
297  std::string goalFrameId_;
298 
301 
330 #ifdef WITH_OCTOMAP_MSGS
331  ros::ServiceServer octomapBinarySrv_;
332  ros::ServiceServer octomapFullSrv_;
333 #endif
334 
336 
337  boost::thread* transformThread_;
339 
340  // for loop closure detection only
342 
344  cv::Mat userData_;
346 
348  geometry_msgs::PoseWithCovarianceStamped globalPose_;
352  std::map<int, geometry_msgs::PoseWithCovarianceStamped> tags_;
354  std::map<double, rtabmap::Transform> imus_;
355  std::string imuFrameId_;
356 
358  std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> > interOdoms_;
363 
366  float rate_;
371 };
372 
373 }
374 
375 #endif /* COREWRAPPER_H_ */
376 
geometry_msgs::PoseWithCovarianceStamped globalPose_
Definition: CoreWrapper.h:348
ros::ServiceServer setLogErrorSrv_
Definition: CoreWrapper.h:313
ros::ServiceServer getMapDataSrv_
Definition: CoreWrapper.h:315
ros::Subscriber gpsFixAsyncSub_
Definition: CoreWrapper.h:349
std::map< int, geometry_msgs::PoseWithCovarianceStamped > tags_
Definition: CoreWrapper.h:352
ros::ServiceServer getNodesInRadiusSrv_
Definition: CoreWrapper.h:329
ros::ServiceServer getProjMapSrv_
Definition: CoreWrapper.h:317
ros::ServiceServer pauseSrv_
Definition: CoreWrapper.h:304
ros::Publisher localPathNodesPub_
Definition: CoreWrapper.h:296
ros::ServiceServer setLogDebugSrv_
Definition: CoreWrapper.h:310
rtabmap::Transform lastPose_
Definition: CoreWrapper.h:236
ros::Publisher globalPathNodesPub_
Definition: CoreWrapper.h:295
ros::ServiceServer publishMapDataSrv_
Definition: CoreWrapper.h:321
ros::ServiceServer setLogWarnSrv_
Definition: CoreWrapper.h:312
image_transport::Subscriber defaultSub_
Definition: CoreWrapper.h:341
ros::ServiceServer resetSrv_
Definition: CoreWrapper.h:303
rtabmap::ParametersMap parameters_
Definition: CoreWrapper.h:243
ros::ServiceServer addLinkSrv_
Definition: CoreWrapper.h:328
ros::Subscriber tagDetectionsSub_
Definition: CoreWrapper.h:351
ros::Publisher localGridEmpty_
Definition: CoreWrapper.h:283
ros::ServiceServer getNodeDataSrv_
Definition: CoreWrapper.h:314
ros::ServiceServer getMapSrv_
Definition: CoreWrapper.h:318
ros::ServiceServer getGridMapSrv_
Definition: CoreWrapper.h:320
rtabmap::Rtabmap rtabmap_
Definition: CoreWrapper.h:234
ros::Publisher localizationPosePub_
Definition: CoreWrapper.h:285
rtabmap::Transform lastPublishedMetricGoal_
Definition: CoreWrapper.h:241
message_filters::Synchronizer< MyExactInterOdomSyncPolicy > * interOdomSync_
Definition: CoreWrapper.h:362
std::map< std::string, std::string > ParametersMap
ros::Publisher infoPub_
Definition: CoreWrapper.h:276
message_filters::sync_policies::ExactTime< nav_msgs::Odometry, rtabmap_ros::OdomInfo > MyExactInterOdomSyncPolicy
Definition: CoreWrapper.h:361
ros::ServiceServer getProbMapSrv_
Definition: CoreWrapper.h:319
ros::Publisher landmarksPub_
Definition: CoreWrapper.h:279
ros::Subscriber imuSub_
Definition: CoreWrapper.h:353
message_filters::Subscriber< rtabmap_ros::OdomInfo > interOdomInfoSyncSub_
Definition: CoreWrapper.h:360
ros::ServiceServer getMapData2Srv_
Definition: CoreWrapper.h:316
ros::Publisher nextMetricGoalPub_
Definition: CoreWrapper.h:291
ros::Subscriber initialPoseSub_
Definition: CoreWrapper.h:286
ros::Publisher mapDataPub_
Definition: CoreWrapper.h:277
ros::Subscriber globalPoseAsyncSub_
Definition: CoreWrapper.h:347
ros::Subscriber goalSub_
Definition: CoreWrapper.h:289
ros::ServiceServer setModeMappingSrv_
Definition: CoreWrapper.h:309
ros::Subscriber userDataAsyncSub_
Definition: CoreWrapper.h:343
ros::ServiceServer updateSrv_
Definition: CoreWrapper.h:302
ros::ServiceServer backupDatabase_
Definition: CoreWrapper.h:307
ros::ServiceServer setLogInfoSrv_
Definition: CoreWrapper.h:311
std::map< std::string, float > rtabmapROSStats_
Definition: CoreWrapper.h:244
ros::Subscriber interOdomSub_
Definition: CoreWrapper.h:357
ros::ServiceServer setLabelSrv_
Definition: CoreWrapper.h:326
tf2_ros::TransformBroadcaster tfBroadcaster_
Definition: CoreWrapper.h:299
ros::Publisher localGridObstacle_
Definition: CoreWrapper.h:282
message_filters::Subscriber< nav_msgs::Odometry > interOdomSyncSub_
Definition: CoreWrapper.h:359
MoveBaseClient * mbClient_
Definition: CoreWrapper.h:335
boost::mutex mapToOdomMutex_
Definition: CoreWrapper.h:272
ros::ServiceServer cancelGoalSrv_
Definition: CoreWrapper.h:325
tf::TransformListener tfListener_
Definition: CoreWrapper.h:300
RecoveryProgressState state
rtabmap::Transform mapToOdom_
Definition: CoreWrapper.h:271
string frameId
Definition: patrol.py:11
actionlib::SimpleActionClient< move_base_msgs::MoveBaseAction > MoveBaseClient
Definition: CoreWrapper.h:82
boost::thread * transformThread_
Definition: CoreWrapper.h:337
rtabmap::Transform currentMetricGoal_
Definition: CoreWrapper.h:240
ros::Subscriber goalNodeSub_
Definition: CoreWrapper.h:290
ros::ServiceServer listLabelsSrv_
Definition: CoreWrapper.h:327
ros::Publisher labelsPub_
Definition: CoreWrapper.h:280
ros::Publisher mapPathPub_
Definition: CoreWrapper.h:281
ros::ServiceServer getPlanNodesSrv_
Definition: CoreWrapper.h:323
ros::Publisher localPathPub_
Definition: CoreWrapper.h:294
std::string groundTruthFrameId_
Definition: CoreWrapper.h:249
std::map< double, rtabmap::Transform > imus_
Definition: CoreWrapper.h:354
ros::ServiceServer getPlanSrv_
Definition: CoreWrapper.h:322
std::list< std::pair< nav_msgs::Odometry, rtabmap_ros::OdomInfo > > interOdoms_
Definition: CoreWrapper.h:358
ros::ServiceServer setGoalSrv_
Definition: CoreWrapper.h:324
ros::Publisher globalPathPub_
Definition: CoreWrapper.h:293
std::string groundTruthBaseFrameId_
Definition: CoreWrapper.h:250
ros::ServiceServer setModeLocalizationSrv_
Definition: CoreWrapper.h:308
ros::Publisher goalReachedPub_
Definition: CoreWrapper.h:292
ros::ServiceServer triggerNewMapSrv_
Definition: CoreWrapper.h:306
ros::Publisher mapGraphPub_
Definition: CoreWrapper.h:278
ros::Publisher localGridGround_
Definition: CoreWrapper.h:284
ros::ServiceServer resumeSrv_
Definition: CoreWrapper.h:305


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:18