Public Member Functions | Private Types | Private Member Functions | Private Attributes
CoreWrapper Class Reference

#include <CoreWrapper.h>

List of all members.

Public Member Functions

 CoreWrapper (bool deleteDbOnStart)
virtual ~CoreWrapper ()

Private Types

typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan > 
MyDepthScanSyncPolicy
typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan > 
MyDepthScanTFSyncPolicy
typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::CameraInfo > 
MyDepthSyncPolicy
typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo > 
MyDepthTFSyncPolicy
typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
nav_msgs::Odometry > 
MyStereoApproxSyncPolicy
typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo > 
MyStereoApproxTFSyncPolicy
typedef
message_filters::sync_policies::ExactTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
nav_msgs::Odometry > 
MyStereoExactSyncPolicy
typedef
message_filters::sync_policies::ExactTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo > 
MyStereoExactTFSyncPolicy
typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan,
nav_msgs::Odometry > 
MyStereoScanSyncPolicy
typedef
message_filters::sync_policies::ApproximateTime
< sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo,
sensor_msgs::LaserScan > 
MyStereoScanTFSyncPolicy

Private Member Functions

bool backupDatabaseCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)
void commonDepthCallback (const std::string &odomFrameId, const sensor_msgs::ImageConstPtr &imageMsg, const sensor_msgs::ImageConstPtr &imageDepthMsg, const sensor_msgs::CameraInfoConstPtr &camInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg)
bool commonOdomTFUpdate (const ros::Time &stamp)
bool commonOdomUpdate (const nav_msgs::OdometryConstPtr &odomMsg)
void commonStereoCallback (const std::string &odomFrameId, const sensor_msgs::ImageConstPtr &leftImageMsg, const sensor_msgs::ImageConstPtr &rightImageMsg, const sensor_msgs::CameraInfoConstPtr &leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr &rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg)
void defaultCallback (const sensor_msgs::ImageConstPtr &imageMsg)
void depthCallback (const sensor_msgs::ImageConstPtr &imageMsg, const nav_msgs::OdometryConstPtr &odomMsg, const sensor_msgs::ImageConstPtr &imageDepthMsg, const sensor_msgs::CameraInfoConstPtr &camInfoMsg)
void depthScanCallback (const sensor_msgs::ImageConstPtr &imageMsg, const nav_msgs::OdometryConstPtr &odomMsg, const sensor_msgs::ImageConstPtr &imageDepthMsg, const sensor_msgs::CameraInfoConstPtr &camInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg)
void depthScanTFCallback (const sensor_msgs::ImageConstPtr &imageMsg, const sensor_msgs::ImageConstPtr &imageDepthMsg, const sensor_msgs::CameraInfoConstPtr &camInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg)
void depthTFCallback (const sensor_msgs::ImageConstPtr &imageMsg, const sensor_msgs::ImageConstPtr &imageDepthMsg, const sensor_msgs::CameraInfoConstPtr &camInfoMsg)
bool getGridMapCallback (nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
bool getMapCallback (rtabmap_ros::GetMap::Request &req, rtabmap_ros::GetMap::Response &res)
bool getProjMapCallback (nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
rtabmap::Transform getTransform (const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp) const
void goalActiveCb ()
void goalCallback (const geometry_msgs::PoseStampedConstPtr &msg)
void goalCommonCallback (const std::vector< std::pair< int, rtabmap::Transform > > &poses)
void goalDoneCb (const actionlib::SimpleClientGoalState &state, const move_base_msgs::MoveBaseResultConstPtr &result)
void goalFeedbackCb (const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
void goalGlobalCallback (const geometry_msgs::PoseStampedConstPtr &msg)
bool listLabelsCallback (rtabmap_ros::ListLabels::Request &req, rtabmap_ros::ListLabels::Response &res)
rtabmap::ParametersMap loadParameters (const std::string &configFile)
bool pauseRtabmapCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)
void process (int id, const ros::Time &stamp, const cv::Mat &image, const rtabmap::Transform &odom=rtabmap::Transform(), const std::string &odomFrameId="", float odomRotationalVariance=1.0f, float odomTransitionalVariance=1.0f, const cv::Mat &depthOrRightImage=cv::Mat(), float fx=0.0f, float fyOrBaseline=0.0f, float cx=0.0f, float cy=0.0f, const rtabmap::Transform &localTransform=rtabmap::Transform(), const cv::Mat &scan=cv::Mat(), int scanMaxPts=0)
void publishCurrentGoal (const ros::Time &stamp)
void publishLocalPath (const ros::Time &stamp)
void publishLoop (double tfDelay)
bool publishMapCallback (rtabmap_ros::PublishMap::Request &, rtabmap_ros::PublishMap::Response &)
void publishMaps (const std::map< int, rtabmap::Transform > &poses, const ros::Time &stamp)
void publishStats (const ros::Time &stamp)
bool resetRtabmapCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)
bool resumeRtabmapCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)
void saveParameters (const std::string &configFile)
bool setGoalCallback (rtabmap_ros::SetGoal::Request &req, rtabmap_ros::SetGoal::Response &res)
bool setLabelCallback (rtabmap_ros::SetLabel::Request &req, rtabmap_ros::SetLabel::Response &res)
bool setModeLocalizationCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)
bool setModeMappingCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)
void setupCallbacks (bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize, bool stereoApproxSync)
void stereoCallback (const sensor_msgs::ImageConstPtr &leftImageMsg, const sensor_msgs::ImageConstPtr &rightImageMsg, const sensor_msgs::CameraInfoConstPtr &leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr &rightCamInfoMsg, const nav_msgs::OdometryConstPtr &odomMsg)
void stereoScanCallback (const sensor_msgs::ImageConstPtr &leftImageMsg, const sensor_msgs::ImageConstPtr &rightImageMsg, const sensor_msgs::CameraInfoConstPtr &leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr &rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg, const nav_msgs::OdometryConstPtr &odomMsg)
void stereoScanTFCallback (const sensor_msgs::ImageConstPtr &leftImageMsg, const sensor_msgs::ImageConstPtr &rightImageMsg, const sensor_msgs::CameraInfoConstPtr &leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr &rightCamInfoMsg, const sensor_msgs::LaserScanConstPtr &scanMsg)
void stereoTFCallback (const sensor_msgs::ImageConstPtr &leftImageMsg, const sensor_msgs::ImageConstPtr &rightImageMsg, const sensor_msgs::CameraInfoConstPtr &leftCamInfoMsg, const sensor_msgs::CameraInfoConstPtr &rightCamInfoMsg)
bool triggerNewMapCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)
void updateGoal (const ros::Time &stamp)
std::map< int, rtabmap::TransformupdateMapCaches (const std::map< int, rtabmap::Transform > &poses, bool updateCloud, bool updateProj, bool updateGrid, const std::map< int, rtabmap::Signature > &signatures=std::map< int, rtabmap::Signature >())
bool updateRtabmapCallback (std_srvs::Empty::Request &, std_srvs::Empty::Response &)

Private Attributes

ros::ServiceServer backupDatabase_
message_filters::Subscriber
< sensor_msgs::CameraInfo > 
cameraInfoLeft_
message_filters::Subscriber
< sensor_msgs::CameraInfo > 
cameraInfoRight_
message_filters::Subscriber
< sensor_msgs::CameraInfo > 
cameraInfoSub_
int cloudDecimation_
ros::Publisher cloudMapPub_
double cloudMaxDepth_
bool cloudOutputVoxelized_
std::map< int, pcl::PointCloud
< pcl::PointXYZRGB >::Ptr > 
clouds_
double cloudVoxelSize_
std::string configPath_
rtabmap::Transform currentMetricGoal_
std::string databasePath_
image_transport::Subscriber defaultSub_
message_filters::Synchronizer
< MyDepthScanSyncPolicy > * 
depthScanSync_
message_filters::Synchronizer
< MyDepthScanTFSyncPolicy > * 
depthScanTFSync_
message_filters::Synchronizer
< MyDepthSyncPolicy > * 
depthSync_
message_filters::Synchronizer
< MyDepthTFSyncPolicy > * 
depthTFSync_
std::string frameId_
ros::ServiceServer getGridMapSrv_
ros::ServiceServer getMapDataSrv_
ros::ServiceServer getProjMapSrv_
ros::Publisher globalPathPub_
ros::Subscriber goalGlobalSub_
ros::Publisher goalReachedPub_
ros::Subscriber goalSub_
double gridCellSize_
bool gridEroded_
ros::Publisher gridMapPub_
std::map< int, std::pair
< cv::Mat, cv::Mat > > 
gridMaps_
double gridSize_
image_transport::SubscriberFilter imageDepthSub_
image_transport::SubscriberFilter imageRectLeft_
image_transport::SubscriberFilter imageRectRight_
image_transport::SubscriberFilter imageSub_
ros::Publisher infoPub_
ros::Publisher labelsPub_
rtabmap::Transform lastPose_
ros::Time lastPoseStamp_
bool latestNodeWasReached_
ros::ServiceServer listLabelsSrv_
ros::Publisher localPathPub_
bool mapCacheCleanup_
ros::Publisher mapDataPub_
double mapFilterAngle_
double mapFilterRadius_
std::string mapFrameId_
ros::Publisher mapGraphPub_
tf::Transform mapToOdom_
boost::mutex mapToOdomMutex_
MoveBaseClient mbClient_
ros::Publisher nextMetricGoalPub_
std::string odomFrameId_
message_filters::Subscriber
< nav_msgs::Odometry > 
odomSub_
rtabmap::ParametersMap parameters_
bool paused_
ros::ServiceServer pauseSrv_
ros::Publisher projMapPub_
std::map< int, std::pair
< cv::Mat, cv::Mat > > 
projMaps_
double projMaxGroundAngle_
double projMaxHeight_
int projMinClusterSize_
ros::ServiceServer publishMapDataSrv_
float rate_
ros::ServiceServer resetSrv_
ros::ServiceServer resumeSrv_
float rotVariance_
rtabmap::Rtabmap rtabmap_
message_filters::Subscriber
< sensor_msgs::LaserScan > 
scanSub_
ros::ServiceServer setGoalSrv_
ros::ServiceServer setLabelSrv_
ros::ServiceServer setModeLocalizationSrv_
ros::ServiceServer setModeMappingSrv_
message_filters::Synchronizer
< MyStereoApproxSyncPolicy > * 
stereoApproxSync_
message_filters::Synchronizer
< MyStereoApproxTFSyncPolicy > * 
stereoApproxTFSync_
message_filters::Synchronizer
< MyStereoExactSyncPolicy > * 
stereoExactSync_
message_filters::Synchronizer
< MyStereoExactTFSyncPolicy > * 
stereoExactTFSync_
message_filters::Synchronizer
< MyStereoScanSyncPolicy > * 
stereoScanSync_
message_filters::Synchronizer
< MyStereoScanTFSyncPolicy > * 
stereoScanTFSync_
tf::TransformBroadcaster tfBroadcaster_
tf::TransformListener tfListener_
ros::Time time_
boost::thread * transformThread_
float transVariance_
ros::ServiceServer triggerNewMapSrv_
ros::ServiceServer updateSrv_
bool useActionForGoal_
bool waitForTransform_

Detailed Description

Definition at line 88 of file CoreWrapper.h.


Member Typedef Documentation

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> CoreWrapper::MyDepthScanSyncPolicy [private]

Definition at line 308 of file CoreWrapper.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> CoreWrapper::MyDepthScanTFSyncPolicy [private]

Definition at line 348 of file CoreWrapper.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, nav_msgs::Odometry, sensor_msgs::Image, sensor_msgs::CameraInfo> CoreWrapper::MyDepthSyncPolicy [private]

Definition at line 315 of file CoreWrapper.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> CoreWrapper::MyDepthTFSyncPolicy [private]

Definition at line 354 of file CoreWrapper.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, nav_msgs::Odometry> CoreWrapper::MyStereoApproxSyncPolicy [private]

Definition at line 332 of file CoreWrapper.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> CoreWrapper::MyStereoApproxTFSyncPolicy [private]

Definition at line 369 of file CoreWrapper.h.

typedef message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, nav_msgs::Odometry> CoreWrapper::MyStereoExactSyncPolicy [private]

Definition at line 340 of file CoreWrapper.h.

typedef message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> CoreWrapper::MyStereoExactTFSyncPolicy [private]

Definition at line 376 of file CoreWrapper.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, sensor_msgs::LaserScan, nav_msgs::Odometry> CoreWrapper::MyStereoScanSyncPolicy [private]

Definition at line 324 of file CoreWrapper.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> CoreWrapper::MyStereoScanTFSyncPolicy [private]

Definition at line 362 of file CoreWrapper.h.


Constructor & Destructor Documentation

CoreWrapper::CoreWrapper ( bool  deleteDbOnStart)

Definition at line 84 of file CoreWrapper.cpp.

Definition at line 406 of file CoreWrapper.cpp.


Member Function Documentation

bool CoreWrapper::backupDatabaseCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1431 of file CoreWrapper.cpp.

void CoreWrapper::commonDepthCallback ( const std::string &  odomFrameId,
const sensor_msgs::ImageConstPtr &  imageMsg,
const sensor_msgs::ImageConstPtr &  imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr &  camInfoMsg,
const sensor_msgs::LaserScanConstPtr &  scanMsg 
) [private]

Definition at line 692 of file CoreWrapper.cpp.

bool CoreWrapper::commonOdomTFUpdate ( const ros::Time stamp) [private]

Definition at line 615 of file CoreWrapper.cpp.

bool CoreWrapper::commonOdomUpdate ( const nav_msgs::OdometryConstPtr &  odomMsg) [private]

Definition at line 575 of file CoreWrapper.cpp.

void CoreWrapper::commonStereoCallback ( const std::string &  odomFrameId,
const sensor_msgs::ImageConstPtr &  leftImageMsg,
const sensor_msgs::ImageConstPtr &  rightImageMsg,
const sensor_msgs::CameraInfoConstPtr &  leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr &  rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr &  scanMsg 
) [private]

Definition at line 810 of file CoreWrapper.cpp.

void CoreWrapper::defaultCallback ( const sensor_msgs::ImageConstPtr &  imageMsg) [private]

Definition at line 513 of file CoreWrapper.cpp.

void CoreWrapper::depthCallback ( const sensor_msgs::ImageConstPtr &  imageMsg,
const nav_msgs::OdometryConstPtr &  odomMsg,
const sensor_msgs::ImageConstPtr &  imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr &  camInfoMsg 
) [private]

Definition at line 931 of file CoreWrapper.cpp.

void CoreWrapper::depthScanCallback ( const sensor_msgs::ImageConstPtr &  imageMsg,
const nav_msgs::OdometryConstPtr &  odomMsg,
const sensor_msgs::ImageConstPtr &  imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr &  camInfoMsg,
const sensor_msgs::LaserScanConstPtr &  scanMsg 
) [private]

Definition at line 945 of file CoreWrapper.cpp.

void CoreWrapper::depthScanTFCallback ( const sensor_msgs::ImageConstPtr &  imageMsg,
const sensor_msgs::ImageConstPtr &  imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr &  camInfoMsg,
const sensor_msgs::LaserScanConstPtr &  scanMsg 
) [private]

Definition at line 1001 of file CoreWrapper.cpp.

void CoreWrapper::depthTFCallback ( const sensor_msgs::ImageConstPtr &  imageMsg,
const sensor_msgs::ImageConstPtr &  imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr &  camInfoMsg 
) [private]

Definition at line 989 of file CoreWrapper.cpp.

bool CoreWrapper::getGridMapCallback ( nav_msgs::GetMap::Request &  req,
nav_msgs::GetMap::Response &  res 
) [private]

Definition at line 1592 of file CoreWrapper.cpp.

bool CoreWrapper::getMapCallback ( rtabmap_ros::GetMap::Request &  req,
rtabmap_ros::GetMap::Response &  res 
) [private]

Definition at line 1472 of file CoreWrapper.cpp.

bool CoreWrapper::getProjMapCallback ( nav_msgs::GetMap::Request &  req,
nav_msgs::GetMap::Response &  res 
) [private]

Definition at line 1542 of file CoreWrapper.cpp.

Transform CoreWrapper::getTransform ( const std::string &  fromFrameId,
const std::string &  toFrameId,
const ros::Time stamp 
) const [private]

Definition at line 666 of file CoreWrapper.cpp.

void CoreWrapper::goalActiveCb ( ) [private]

Definition at line 2558 of file CoreWrapper.cpp.

void CoreWrapper::goalCallback ( const geometry_msgs::PoseStampedConstPtr &  msg) [private]

Definition at line 1306 of file CoreWrapper.cpp.

void CoreWrapper::goalCommonCallback ( const std::vector< std::pair< int, rtabmap::Transform > > &  poses) [private]

Definition at line 1228 of file CoreWrapper.cpp.

void CoreWrapper::goalDoneCb ( const actionlib::SimpleClientGoalState state,
const move_base_msgs::MoveBaseResultConstPtr &  result 
) [private]

Definition at line 2514 of file CoreWrapper.cpp.

void CoreWrapper::goalFeedbackCb ( const move_base_msgs::MoveBaseFeedbackConstPtr &  feedback) [private]

Definition at line 2564 of file CoreWrapper.cpp.

void CoreWrapper::goalGlobalCallback ( const geometry_msgs::PoseStampedConstPtr &  msg) [private]

Definition at line 1321 of file CoreWrapper.cpp.

bool CoreWrapper::listLabelsCallback ( rtabmap_ros::ListLabels::Request &  req,
rtabmap_ros::ListLabels::Response &  res 
) [private]

Definition at line 1916 of file CoreWrapper.cpp.

ParametersMap CoreWrapper::loadParameters ( const std::string &  configFile) [private]

Definition at line 448 of file CoreWrapper.cpp.

bool CoreWrapper::pauseRtabmapCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1392 of file CoreWrapper.cpp.

void CoreWrapper::process ( int  id,
const ros::Time stamp,
const cv::Mat &  image,
const rtabmap::Transform odom = rtabmap::Transform(),
const std::string &  odomFrameId = "",
float  odomRotationalVariance = 1.0f,
float  odomTransitionalVariance = 1.0f,
const cv::Mat &  depthOrRightImage = cv::Mat(),
float  fx = 0.0f,
float  fyOrBaseline = 0.0f,
float  cx = 0.0f,
float  cy = 0.0f,
const rtabmap::Transform localTransform = rtabmap::Transform(),
const cv::Mat &  scan = cv::Mat(),
int  scanMaxPts = 0 
) [private]

Definition at line 1041 of file CoreWrapper.cpp.

void CoreWrapper::publishCurrentGoal ( const ros::Time stamp) [private]

Definition at line 2471 of file CoreWrapper.cpp.

void CoreWrapper::publishLocalPath ( const ros::Time stamp) [private]

Definition at line 2570 of file CoreWrapper.cpp.

void CoreWrapper::publishLoop ( double  tfDelay) [private]

Definition at line 495 of file CoreWrapper.cpp.

bool CoreWrapper::publishMapCallback ( rtabmap_ros::PublishMap::Request &  req,
rtabmap_ros::PublishMap::Response &  res 
) [private]

Definition at line 1642 of file CoreWrapper.cpp.

void CoreWrapper::publishMaps ( const std::map< int, rtabmap::Transform > &  poses,
const ros::Time stamp 
) [private]

Definition at line 2338 of file CoreWrapper.cpp.

void CoreWrapper::publishStats ( const ros::Time stamp) [private]

Definition at line 1927 of file CoreWrapper.cpp.

bool CoreWrapper::resetRtabmapCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1377 of file CoreWrapper.cpp.

bool CoreWrapper::resumeRtabmapCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1408 of file CoreWrapper.cpp.

void CoreWrapper::saveParameters ( const std::string &  configFile) [private]

Definition at line 465 of file CoreWrapper.cpp.

bool CoreWrapper::setGoalCallback ( rtabmap_ros::SetGoal::Request &  req,
rtabmap_ros::SetGoal::Response &  res 
) [private]

Definition at line 1858 of file CoreWrapper.cpp.

bool CoreWrapper::setLabelCallback ( rtabmap_ros::SetLabel::Request &  req,
rtabmap_ros::SetLabel::Response &  res 
) [private]

Definition at line 1889 of file CoreWrapper.cpp.

bool CoreWrapper::setModeLocalizationCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1454 of file CoreWrapper.cpp.

bool CoreWrapper::setModeMappingCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1463 of file CoreWrapper.cpp.

void CoreWrapper::setupCallbacks ( bool  subscribeDepth,
bool  subscribeLaserScan,
bool  subscribeStereo,
int  queueSize,
bool  stereoApproxSync 
) [private]

exclusive callbacks: image image + depth image + scan image + depth + scan Which callback is called depends on the combination of these options: bool subscribe_laserScan bool subscribe_depth

subscribeLaserScan

subscribeLaserScan

subscribeLaserScan

subscribeLaserScan

Definition at line 2699 of file CoreWrapper.cpp.

void CoreWrapper::stereoCallback ( const sensor_msgs::ImageConstPtr &  leftImageMsg,
const sensor_msgs::ImageConstPtr &  rightImageMsg,
const sensor_msgs::CameraInfoConstPtr &  leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr &  rightCamInfoMsg,
const nav_msgs::OdometryConstPtr &  odomMsg 
) [private]

Definition at line 958 of file CoreWrapper.cpp.

void CoreWrapper::stereoScanCallback ( const sensor_msgs::ImageConstPtr &  leftImageMsg,
const sensor_msgs::ImageConstPtr &  rightImageMsg,
const sensor_msgs::CameraInfoConstPtr &  leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr &  rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr &  scanMsg,
const nav_msgs::OdometryConstPtr &  odomMsg 
) [private]

Definition at line 973 of file CoreWrapper.cpp.

void CoreWrapper::stereoScanTFCallback ( const sensor_msgs::ImageConstPtr &  leftImageMsg,
const sensor_msgs::ImageConstPtr &  rightImageMsg,
const sensor_msgs::CameraInfoConstPtr &  leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr &  rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr &  scanMsg 
) [private]

Definition at line 1027 of file CoreWrapper.cpp.

void CoreWrapper::stereoTFCallback ( const sensor_msgs::ImageConstPtr &  leftImageMsg,
const sensor_msgs::ImageConstPtr &  rightImageMsg,
const sensor_msgs::CameraInfoConstPtr &  leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr &  rightCamInfoMsg 
) [private]

Definition at line 1013 of file CoreWrapper.cpp.

bool CoreWrapper::triggerNewMapCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1424 of file CoreWrapper.cpp.

void CoreWrapper::updateGoal ( const ros::Time stamp) [private]
std::map< int, rtabmap::Transform > CoreWrapper::updateMapCaches ( const std::map< int, rtabmap::Transform > &  poses,
bool  updateCloud,
bool  updateProj,
bool  updateGrid,
const std::map< int, rtabmap::Signature > &  signatures = std::map<int, rtabmap::Signature>() 
) [private]

Definition at line 2068 of file CoreWrapper.cpp.

bool CoreWrapper::updateRtabmapCallback ( std_srvs::Empty::Request &  ,
std_srvs::Empty::Response &   
) [private]

Definition at line 1336 of file CoreWrapper.cpp.


Member Data Documentation

Definition at line 387 of file CoreWrapper.h.

message_filters::Subscriber<sensor_msgs::CameraInfo> CoreWrapper::cameraInfoLeft_ [private]

Definition at line 297 of file CoreWrapper.h.

message_filters::Subscriber<sensor_msgs::CameraInfo> CoreWrapper::cameraInfoRight_ [private]

Definition at line 298 of file CoreWrapper.h.

message_filters::Subscriber<sensor_msgs::CameraInfo> CoreWrapper::cameraInfoSub_ [private]

Definition at line 292 of file CoreWrapper.h.

Definition at line 249 of file CoreWrapper.h.

Definition at line 274 of file CoreWrapper.h.

double CoreWrapper::cloudMaxDepth_ [private]

Definition at line 250 of file CoreWrapper.h.

Definition at line 252 of file CoreWrapper.h.

std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > CoreWrapper::clouds_ [private]

Definition at line 266 of file CoreWrapper.h.

double CoreWrapper::cloudVoxelSize_ [private]

Definition at line 251 of file CoreWrapper.h.

std::string CoreWrapper::configPath_ [private]

Definition at line 243 of file CoreWrapper.h.

Definition at line 236 of file CoreWrapper.h.

std::string CoreWrapper::databasePath_ [private]

Definition at line 244 of file CoreWrapper.h.

Definition at line 287 of file CoreWrapper.h.

Definition at line 309 of file CoreWrapper.h.

Definition at line 349 of file CoreWrapper.h.

Definition at line 316 of file CoreWrapper.h.

Definition at line 355 of file CoreWrapper.h.

std::string CoreWrapper::frameId_ [private]

Definition at line 240 of file CoreWrapper.h.

Definition at line 392 of file CoreWrapper.h.

Definition at line 390 of file CoreWrapper.h.

Definition at line 391 of file CoreWrapper.h.

Definition at line 283 of file CoreWrapper.h.

Definition at line 280 of file CoreWrapper.h.

Definition at line 282 of file CoreWrapper.h.

Definition at line 279 of file CoreWrapper.h.

double CoreWrapper::gridCellSize_ [private]

Definition at line 256 of file CoreWrapper.h.

bool CoreWrapper::gridEroded_ [private]

Definition at line 258 of file CoreWrapper.h.

Definition at line 276 of file CoreWrapper.h.

std::map<int, std::pair<cv::Mat, cv::Mat> > CoreWrapper::gridMaps_ [private]

Definition at line 268 of file CoreWrapper.h.

double CoreWrapper::gridSize_ [private]

Definition at line 257 of file CoreWrapper.h.

Definition at line 291 of file CoreWrapper.h.

Definition at line 295 of file CoreWrapper.h.

Definition at line 296 of file CoreWrapper.h.

Definition at line 290 of file CoreWrapper.h.

Definition at line 270 of file CoreWrapper.h.

Definition at line 273 of file CoreWrapper.h.

Definition at line 232 of file CoreWrapper.h.

Definition at line 233 of file CoreWrapper.h.

Definition at line 237 of file CoreWrapper.h.

Definition at line 396 of file CoreWrapper.h.

Definition at line 284 of file CoreWrapper.h.

Definition at line 261 of file CoreWrapper.h.

Definition at line 271 of file CoreWrapper.h.

double CoreWrapper::mapFilterAngle_ [private]

Definition at line 260 of file CoreWrapper.h.

Definition at line 259 of file CoreWrapper.h.

std::string CoreWrapper::mapFrameId_ [private]

Definition at line 241 of file CoreWrapper.h.

Definition at line 272 of file CoreWrapper.h.

Definition at line 263 of file CoreWrapper.h.

boost::mutex CoreWrapper::mapToOdomMutex_ [private]

Definition at line 264 of file CoreWrapper.h.

Definition at line 402 of file CoreWrapper.h.

Definition at line 281 of file CoreWrapper.h.

std::string CoreWrapper::odomFrameId_ [private]

Definition at line 242 of file CoreWrapper.h.

message_filters::Subscriber<nav_msgs::Odometry> CoreWrapper::odomSub_ [private]

Definition at line 300 of file CoreWrapper.h.

Definition at line 238 of file CoreWrapper.h.

bool CoreWrapper::paused_ [private]

Definition at line 231 of file CoreWrapper.h.

Definition at line 384 of file CoreWrapper.h.

Definition at line 275 of file CoreWrapper.h.

std::map<int, std::pair<cv::Mat, cv::Mat> > CoreWrapper::projMaps_ [private]

Definition at line 267 of file CoreWrapper.h.

Definition at line 253 of file CoreWrapper.h.

double CoreWrapper::projMaxHeight_ [private]

Definition at line 255 of file CoreWrapper.h.

Definition at line 254 of file CoreWrapper.h.

Definition at line 393 of file CoreWrapper.h.

float CoreWrapper::rate_ [private]

Definition at line 406 of file CoreWrapper.h.

Definition at line 383 of file CoreWrapper.h.

Definition at line 385 of file CoreWrapper.h.

float CoreWrapper::rotVariance_ [private]

Definition at line 234 of file CoreWrapper.h.

Definition at line 230 of file CoreWrapper.h.

message_filters::Subscriber<sensor_msgs::LaserScan> CoreWrapper::scanSub_ [private]

Definition at line 301 of file CoreWrapper.h.

Definition at line 394 of file CoreWrapper.h.

Definition at line 395 of file CoreWrapper.h.

Definition at line 388 of file CoreWrapper.h.

Definition at line 389 of file CoreWrapper.h.

Definition at line 333 of file CoreWrapper.h.

Definition at line 370 of file CoreWrapper.h.

Definition at line 341 of file CoreWrapper.h.

Definition at line 377 of file CoreWrapper.h.

Definition at line 325 of file CoreWrapper.h.

Definition at line 363 of file CoreWrapper.h.

Definition at line 379 of file CoreWrapper.h.

Definition at line 380 of file CoreWrapper.h.

Definition at line 407 of file CoreWrapper.h.

boost::thread* CoreWrapper::transformThread_ [private]

Definition at line 404 of file CoreWrapper.h.

float CoreWrapper::transVariance_ [private]

Definition at line 235 of file CoreWrapper.h.

Definition at line 386 of file CoreWrapper.h.

Definition at line 382 of file CoreWrapper.h.

Definition at line 246 of file CoreWrapper.h.

Definition at line 245 of file CoreWrapper.h.


The documentation for this class was generated from the following files:


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Thu Aug 27 2015 15:00:25