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 #ifndef COREWRAPPER_H_
00029 #define COREWRAPPER_H_
00030
00031
00032 #include <ros/ros.h>
00033 #include <nodelet/nodelet.h>
00034
00035 #include <std_srvs/Empty.h>
00036
00037 #include <tf/transform_listener.h>
00038 #include <tf2_ros/transform_broadcaster.h>
00039
00040 #include <std_msgs/Empty.h>
00041 #include <std_msgs/Int32.h>
00042 #include <nav_msgs/GetMap.h>
00043 #include <nav_msgs/GetPlan.h>
00044 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00045
00046 #include <rtabmap/core/Parameters.h>
00047 #include <rtabmap/core/Rtabmap.h>
00048 #include <rtabmap/core/OdometryInfo.h>
00049
00050 #include "rtabmap_ros/GetMap.h"
00051 #include "rtabmap_ros/ListLabels.h"
00052 #include "rtabmap_ros/PublishMap.h"
00053 #include "rtabmap_ros/SetGoal.h"
00054 #include "rtabmap_ros/SetLabel.h"
00055 #include "rtabmap_ros/Goal.h"
00056 #include "rtabmap_ros/CommonDataSubscriber.h"
00057
00058 #include "MapsManager.h"
00059
00060 #ifdef WITH_OCTOMAP_MSGS
00061 #include <octomap_msgs/GetOctomap.h>
00062 #endif
00063
00064 #include <actionlib/client/simple_action_client.h>
00065 #include <move_base_msgs/MoveBaseAction.h>
00066 #include <move_base_msgs/MoveBaseActionGoal.h>
00067 #include <move_base_msgs/MoveBaseActionResult.h>
00068 #include <move_base_msgs/MoveBaseActionFeedback.h>
00069 #include <actionlib_msgs/GoalStatusArray.h>
00070 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00071
00072 namespace rtabmap {
00073 class StereoDense;
00074 }
00075
00076 namespace rtabmap_ros {
00077
00078 class CoreWrapper : public CommonDataSubscriber, public nodelet::Nodelet
00079 {
00080 public:
00081 CoreWrapper();
00082 virtual ~CoreWrapper();
00083
00084 private:
00085
00086 virtual void onInit();
00087
00088 bool odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Time stamp);
00089 bool odomTFUpdate(const ros::Time & stamp);
00090
00091 virtual void commonDepthCallback(
00092 const nav_msgs::OdometryConstPtr & odomMsg,
00093 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00094 const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
00095 const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
00096 const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
00097 const sensor_msgs::LaserScanConstPtr& scanMsg,
00098 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
00099 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
00100 void commonDepthCallbackImpl(
00101 const std::string & odomFrameId,
00102 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00103 const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
00104 const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
00105 const std::vector<sensor_msgs::CameraInfo> & cameraInfoMsgs,
00106 const sensor_msgs::LaserScanConstPtr& scan2dMsg,
00107 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
00108 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
00109 virtual void commonStereoCallback(
00110 const nav_msgs::OdometryConstPtr & odomMsg,
00111 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00112 const cv_bridge::CvImageConstPtr& leftImageMsg,
00113 const cv_bridge::CvImageConstPtr& rightImageMsg,
00114 const sensor_msgs::CameraInfo& leftCamInfoMsg,
00115 const sensor_msgs::CameraInfo& rightCamInfoMsg,
00116 const sensor_msgs::LaserScanConstPtr& scanMsg,
00117 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
00118 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
00119
00120 void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg);
00121
00122 void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
00123 void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg);
00124
00125 void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg);
00126
00127 void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
00128 void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
00129 void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
00130 void updateGoal(const ros::Time & stamp);
00131
00132 void process(
00133 const ros::Time & stamp,
00134 const rtabmap::SensorData & data,
00135 const rtabmap::Transform & odom = rtabmap::Transform(),
00136 const std::string & odomFrameId = "",
00137 const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
00138 const rtabmap::OdometryInfo & odomInfo = rtabmap::OdometryInfo());
00139
00140 bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00141 bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00142 bool pauseRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00143 bool resumeRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00144 bool triggerNewMapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00145 bool backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00146 bool setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00147 bool setModeMappingCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00148 bool setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00149 bool setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00150 bool setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00151 bool setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00152 bool getMapDataCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros::GetMap::Response& res);
00153 bool getMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
00154 bool getProbMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
00155 bool getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
00156 bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
00157 bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
00158 bool getPlanCallback(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &res);
00159 bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
00160 bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00161 bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
00162 bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
00163 #ifdef WITH_OCTOMAP_MSGS
00164 bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
00165 bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
00166 #endif
00167
00168 void loadParameters(const std::string & configFile, rtabmap::ParametersMap & parameters);
00169 void saveParameters(const std::string & configFile);
00170
00171 void publishLoop(double tfDelay, double tfTolerance);
00172
00173 void publishStats(const ros::Time & stamp);
00174 void publishCurrentGoal(const ros::Time & stamp);
00175 void goalDoneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result);
00176 void goalActiveCb();
00177 void goalFeedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback);
00178 void publishLocalPath(const ros::Time & stamp);
00179 void publishGlobalPath(const ros::Time & stamp);
00180
00181 private:
00182 rtabmap::Rtabmap rtabmap_;
00183 bool paused_;
00184 rtabmap::Transform lastPose_;
00185 ros::Time lastPoseStamp_;
00186 bool lastPoseIntermediate_;
00187 cv::Mat covariance_;
00188 rtabmap::Transform currentMetricGoal_;
00189 rtabmap::Transform lastPublishedMetricGoal_;
00190 bool latestNodeWasReached_;
00191 rtabmap::ParametersMap parameters_;
00192 std::map<std::string, float> rtabmapROSStats_;
00193
00194 std::string frameId_;
00195 std::string odomFrameId_;
00196 std::string mapFrameId_;
00197 std::string groundTruthFrameId_;
00198 std::string groundTruthBaseFrameId_;
00199 std::string configPath_;
00200 std::string databasePath_;
00201 double odomDefaultAngVariance_;
00202 double odomDefaultLinVariance_;
00203 bool waitForTransform_;
00204 double waitForTransformDuration_;
00205 bool useActionForGoal_;
00206 bool useSavedMap_;
00207 bool genScan_;
00208 double genScanMaxDepth_;
00209 double genScanMinDepth_;
00210 int scanCloudMaxPoints_;
00211
00212 rtabmap::Transform mapToOdom_;
00213 boost::mutex mapToOdomMutex_;
00214
00215 MapsManager mapsManager_;
00216
00217 ros::Publisher infoPub_;
00218 ros::Publisher mapDataPub_;
00219 ros::Publisher mapGraphPub_;
00220 ros::Publisher labelsPub_;
00221 ros::Publisher mapPathPub_;
00222 ros::Publisher localizationPosePub_;
00223 ros::Subscriber initialPoseSub_;
00224
00225
00226 ros::Subscriber goalSub_;
00227 ros::Subscriber goalNodeSub_;
00228 ros::Publisher nextMetricGoalPub_;
00229 ros::Publisher goalReachedPub_;
00230 ros::Publisher globalPathPub_;
00231 ros::Publisher localPathPub_;
00232
00233 tf2_ros::TransformBroadcaster tfBroadcaster_;
00234 tf::TransformListener tfListener_;
00235
00236 ros::ServiceServer updateSrv_;
00237 ros::ServiceServer resetSrv_;
00238 ros::ServiceServer pauseSrv_;
00239 ros::ServiceServer resumeSrv_;
00240 ros::ServiceServer triggerNewMapSrv_;
00241 ros::ServiceServer backupDatabase_;
00242 ros::ServiceServer setModeLocalizationSrv_;
00243 ros::ServiceServer setModeMappingSrv_;
00244 ros::ServiceServer setLogDebugSrv_;
00245 ros::ServiceServer setLogInfoSrv_;
00246 ros::ServiceServer setLogWarnSrv_;
00247 ros::ServiceServer setLogErrorSrv_;
00248 ros::ServiceServer getMapDataSrv_;
00249 ros::ServiceServer getProjMapSrv_;
00250 ros::ServiceServer getMapSrv_;
00251 ros::ServiceServer getProbMapSrv_;
00252 ros::ServiceServer getGridMapSrv_;
00253 ros::ServiceServer publishMapDataSrv_;
00254 ros::ServiceServer getPlanSrv_;
00255 ros::ServiceServer setGoalSrv_;
00256 ros::ServiceServer cancelGoalSrv_;
00257 ros::ServiceServer setLabelSrv_;
00258 ros::ServiceServer listLabelsSrv_;
00259 #ifdef WITH_OCTOMAP_MSGS
00260 ros::ServiceServer octomapBinarySrv_;
00261 ros::ServiceServer octomapFullSrv_;
00262 #endif
00263
00264 MoveBaseClient mbClient_;
00265
00266 boost::thread* transformThread_;
00267 bool tfThreadRunning_;
00268
00269
00270 image_transport::Subscriber defaultSub_;
00271
00272
00273 image_transport::SubscriberFilter rgbSub_;
00274 message_filters::Subscriber<nav_msgs::Odometry> rgbOdomSub_;
00275 message_filters::Subscriber<sensor_msgs::CameraInfo> rgbCameraInfoSub_;
00276 DATA_SYNCS2(rgb, sensor_msgs::Image, sensor_msgs::CameraInfo);
00277 DATA_SYNCS3(rgbOdom, sensor_msgs::Image, sensor_msgs::CameraInfo, nav_msgs::Odometry);
00278
00279 ros::Subscriber userDataAsyncSub_;
00280 cv::Mat userData_;
00281 UMutex userDataMutex_;
00282
00283 ros::Subscriber globalPoseAsyncSub_;
00284 geometry_msgs::PoseWithCovarianceStamped globalPose_;
00285
00286 bool stereoToDepth_;
00287 bool odomSensorSync_;
00288 float rate_;
00289 bool createIntermediateNodes_;
00290 int maxMappingNodes_;
00291 ros::Time time_;
00292 ros::Time previousStamp_;
00293 };
00294
00295 }
00296
00297 #endif
00298