#include <CoreWrapper.h>
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::Transform > | 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 >()) |
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_ |
Definition at line 88 of file CoreWrapper.h.
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.
CoreWrapper::CoreWrapper | ( | bool | deleteDbOnStart | ) |
Definition at line 84 of file CoreWrapper.cpp.
CoreWrapper::~CoreWrapper | ( | ) | [virtual] |
Definition at line 406 of file CoreWrapper.cpp.
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.
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.
int CoreWrapper::cloudDecimation_ [private] |
Definition at line 249 of file CoreWrapper.h.
ros::Publisher CoreWrapper::cloudMapPub_ [private] |
Definition at line 274 of file CoreWrapper.h.
double CoreWrapper::cloudMaxDepth_ [private] |
Definition at line 250 of file CoreWrapper.h.
bool CoreWrapper::cloudOutputVoxelized_ [private] |
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.
ros::Publisher CoreWrapper::globalPathPub_ [private] |
Definition at line 283 of file CoreWrapper.h.
ros::Subscriber CoreWrapper::goalGlobalSub_ [private] |
Definition at line 280 of file CoreWrapper.h.
ros::Publisher CoreWrapper::goalReachedPub_ [private] |
Definition at line 282 of file CoreWrapper.h.
ros::Subscriber CoreWrapper::goalSub_ [private] |
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.
ros::Publisher CoreWrapper::gridMapPub_ [private] |
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.
ros::Publisher CoreWrapper::infoPub_ [private] |
Definition at line 270 of file CoreWrapper.h.
ros::Publisher CoreWrapper::labelsPub_ [private] |
Definition at line 273 of file CoreWrapper.h.
rtabmap::Transform CoreWrapper::lastPose_ [private] |
Definition at line 232 of file CoreWrapper.h.
ros::Time CoreWrapper::lastPoseStamp_ [private] |
Definition at line 233 of file CoreWrapper.h.
bool CoreWrapper::latestNodeWasReached_ [private] |
Definition at line 237 of file CoreWrapper.h.
Definition at line 396 of file CoreWrapper.h.
ros::Publisher CoreWrapper::localPathPub_ [private] |
Definition at line 284 of file CoreWrapper.h.
bool CoreWrapper::mapCacheCleanup_ [private] |
Definition at line 261 of file CoreWrapper.h.
ros::Publisher CoreWrapper::mapDataPub_ [private] |
Definition at line 271 of file CoreWrapper.h.
double CoreWrapper::mapFilterAngle_ [private] |
Definition at line 260 of file CoreWrapper.h.
double CoreWrapper::mapFilterRadius_ [private] |
Definition at line 259 of file CoreWrapper.h.
std::string CoreWrapper::mapFrameId_ [private] |
Definition at line 241 of file CoreWrapper.h.
ros::Publisher CoreWrapper::mapGraphPub_ [private] |
Definition at line 272 of file CoreWrapper.h.
tf::Transform CoreWrapper::mapToOdom_ [private] |
Definition at line 263 of file CoreWrapper.h.
boost::mutex CoreWrapper::mapToOdomMutex_ [private] |
Definition at line 264 of file CoreWrapper.h.
MoveBaseClient CoreWrapper::mbClient_ [private] |
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.
ros::ServiceServer CoreWrapper::pauseSrv_ [private] |
Definition at line 384 of file CoreWrapper.h.
ros::Publisher CoreWrapper::projMapPub_ [private] |
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.
double CoreWrapper::projMaxGroundAngle_ [private] |
Definition at line 253 of file CoreWrapper.h.
double CoreWrapper::projMaxHeight_ [private] |
Definition at line 255 of file CoreWrapper.h.
int CoreWrapper::projMinClusterSize_ [private] |
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.
ros::ServiceServer CoreWrapper::resetSrv_ [private] |
Definition at line 383 of file CoreWrapper.h.
ros::ServiceServer CoreWrapper::resumeSrv_ [private] |
Definition at line 385 of file CoreWrapper.h.
float CoreWrapper::rotVariance_ [private] |
Definition at line 234 of file CoreWrapper.h.
rtabmap::Rtabmap CoreWrapper::rtabmap_ [private] |
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.
ros::ServiceServer CoreWrapper::setGoalSrv_ [private] |
Definition at line 394 of file CoreWrapper.h.
ros::ServiceServer CoreWrapper::setLabelSrv_ [private] |
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.
message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>* CoreWrapper::stereoApproxTFSync_ [private] |
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.
ros::Time CoreWrapper::time_ [private] |
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.
ros::ServiceServer CoreWrapper::updateSrv_ [private] |
Definition at line 382 of file CoreWrapper.h.
bool CoreWrapper::useActionForGoal_ [private] |
Definition at line 246 of file CoreWrapper.h.
bool CoreWrapper::waitForTransform_ [private] |
Definition at line 245 of file CoreWrapper.h.